fix PWM

3 files changed, 201 insertions, 210 deletions

diff --git a/CODE/CODE.ino b/CODE/CODE.ino
index 0d225a4..f6918a7 100644
--- a/CODE/CODE.ino
+++ b/CODE/CODE.ino
@@ -66,6 +66,10 @@ int dpr6 = "LOW";
 int dpr7 = "LOW";
 int dpr8 = "LOW";
 
+double sp1 = 0.00;
+double sp2 = 0.00;
+double sp3 = 0.00;
+
 void setup() {
   pinMode(m1enA, OUTPUT);
   pinMode(m1enB, OUTPUT);
@@ -98,12 +102,10 @@ void setup() {
   Serial2.begin(9600);
 
   Timer3.initialize(800); // 800 microseconds / 1.25kHz
-  Timer3.attachInterrupt(updatePWM1);
-  Timer3.attachInterrupt(updatePWM2);
-  Timer3.attachInterrupt(updatePWM3);
+  Timer3.attachInterrupt(updatePWM); 
 }
 
-void updatePWM1() {
+void updatePWM() {
   static int counter = 0;
 
   if (counter < PWMval1) {
@@ -111,32 +113,14 @@ void updatePWM1() {
   } else {
     digitalWrite(m1enA, LOW);
   }
-
-  counter++;
-  if (counter >= 100) {
-    counter = 0;
-  }
-}
-
-void updatePWM2() {
-  static int counter = 0;
-
-  if (counter < PWMval1) {
+  
+  if (counter < PWMval2) {
     digitalWrite(m2enA, HIGH);
   } else {
     digitalWrite(m2enA, LOW);
   }
-
-  counter++;
-  if (counter >= 100) {
-    counter = 0;
-  }
-}
-
-void updatePWM3() {
-  static int counter = 0;
-
-  if (counter < PWMval1) {
+  
+  if (counter < PWMval3) {
     digitalWrite(m3enA, HIGH);
   } else {
     digitalWrite(m3enA, LOW);
@@ -152,28 +136,76 @@ void mcpsoftpwm1(int value1) {
   if (value1 < 0) value1 = 0;
   if (value1 > 100) value1 = 100;
 
-  //Serial.print("softPWM pin: 3, value: ");
-  //Serial.println(value1); 
+  Serial.print("softPWM pin: enA, value: ");
+  Serial.println(value1); 
   PWMval1 = value1; 
 }
 void mcpsoftpwm2(int value2) {
   if (value2 < 0) value2 = 0;
   if (value2 > 100) value2 = 100;
 
-  //Serial.print("softPWM pin: 3, value: ");
-  //Serial.println(value2); 
+  Serial.print("softPWM pin: enA, value: ");
+  Serial.println(value2); 
   PWMval2 = value2; 
-}
-void mcpsoftpwm3(int value3) {
+}void mcpsoftpwm3(int value3) {
   if (value3 < 0) value3 = 0;
   if (value3 > 100) value3 = 100;
 
-  //Serial.print("softPWM pin: 3, value: ");
-  //Serial.println(value3); 
+  Serial.print("softPWM pin: enA, value: ");
+  Serial.println(value3); 
   PWMval3 = value3; 
 }
 
-void drive(int speed, int direction){
+void move1(int sped1) {
+  sp1 = abs(sped1)*0.1;
+  mcpsoftpwm1(sp1);
+  digitalWrite(m1enB, LOW);
+  if (sped1 < 0) {
+    Serial.print("back ");
+    digitalWrite(m1pwm1, LOW);
+    digitalWrite(m1pwm2, HIGH);
+  } else {
+    Serial.print("forward ");
+    digitalWrite(m1pwm1, HIGH);
+    digitalWrite(m1pwm2, LOW);
+  } 
+  Serial.println(sp1);
+}
+
+void move2(int sped2) {
+  sp2 = abs(sped2)*0.1;
+  mcpsoftpwm2(sp2);
+  digitalWrite(m2enB, LOW);
+  if (sped2 < 0) {
+    Serial.print("back ");
+    digitalWrite(m2pwm1, LOW);
+    digitalWrite(m2pwm2, HIGH);
+  } else {
+    Serial.print("forward ");
+    digitalWrite(m2pwm1, HIGH);
+    digitalWrite(m2pwm2, LOW);
+  } 
+  Serial.println(sp2);
+}
+
+void move3(int sped3) {
+  sp3 = abs(sped3)*0.1;
+  mcpsoftpwm3(sp3);
+  digitalWrite(m3enB, LOW);
+  if (sped3 < 0) {
+    Serial.print("back ");
+    digitalWrite(m3pwm1, LOW);
+    digitalWrite(m3pwm2, HIGH);
+  } else {
+    Serial.print("forward ");
+    digitalWrite(m3pwm1, HIGH);
+    digitalWrite(m3pwm2, LOW);
+  } 
+  Serial.println(sp3);
+}
+
+
+void drive(int speeeed, int direction){
  //
  // Direction............|.Motor 1.|.Motor 2.|.Motor 3.
  // 0deg(forward)........|....0....|....-....|....+....
@@ -183,76 +215,42 @@ void drive(int speed, int direction){
  // 240 deg..............|....-....|....+....|....0....
  // 300 deg..............|....-....|....0....|....+....
  //
-
-  mcpsoftpwm1(speed);  //pwm speed signal
-  mcpsoftpwm2(speed);  //pwm speed signal
-  mcpsoftpwm2(speed);  //pwm speed signal
-  digitalWrite(m1enB, LOW);
-  digitalWrite(m2enB, LOW);
-  digitalWrite(m3enB, LOW);
-  //Serial.println("debug2");
-  //Serial.println(direction);
-  if(direction>330 || direction<=30){
+ if(direction>330 || direction<=30){
     //0 deg
-    digitalWrite(m1pwm1, LOW);
-    digitalWrite(m1pwm2, HIGH);
-
-    digitalWrite(m2pwm1, LOW);
-    digitalWrite(m2pwm2, LOW);
-
-    digitalWrite(m3pwm1, LOW);
-    digitalWrite(m3pwm2, HIGH);
-    //Serial.println("0 deg"); 
+    //Serial.println("drive 0 deg"); 
+    move1(0);
+    move2(-speeeed);
+    move3(speeeed);
   }else if (direction>30 && direction <= 90){
     //60 deg
-    //Serial.println("60deg");
-    digitalWrite(m1pwm1, LOW);
-    digitalWrite(m1pwm2, LOW);
-
-    digitalWrite(m2pwm1, HIGH);
-    digitalWrite(m2pwm2, LOW);
-    
-    digitalWrite(m3pwm1, LOW);
-    digitalWrite(m3pwm2, HIGH);
+    //Serial.println("drive 60deg");
+    move1(speeeed);
+    move2(-speeeed);
+    move3(0);
   }else if (direction>90 && direction <= 150){
     //120 deg
-    //Serial.println("120 deg");
-    digitalWrite(m1pwm1, HIGH);
-    digitalWrite(m1pwm2, LOW);
-    digitalWrite(m2pwm1, HIGH);
-    digitalWrite(m2pwm2, LOW);
-    digitalWrite(m3pwm1, LOW);
-    digitalWrite(m3pwm2, LOW);
+    //Serial.println("drive 120 deg");
+    move1(speeeed);
+    move2(0);
+    move3(-speeeed);
   }else if (direction > 150 && direction <= 210) {
     //180 degrees
-    //Serial.println("on 180");
-    digitalWrite(m1pwm1, HIGH);
-    digitalWrite(m1pwm2, LOW);
-
-    digitalWrite(m2pwm1, LOW);
-    digitalWrite(m2pwm2, LOW);
-    
-    digitalWrite(m3pwm1, HIGH);
-    digitalWrite(m3pwm2, LOW);
+    //Serial.println("drive  180 deg");
+    move1(0);
+    move2(speeeed);
+    move3(-speeeed);
   }else if (direction > 210 && direction <= 270) {
     //240 degrees
-    digitalWrite(m1pwm1, LOW);
-    digitalWrite(m1pwm2, LOW);
-
-    digitalWrite(m2pwm1, LOW);
-    digitalWrite(m2pwm2, HIGH);
-
-    digitalWrite(m3pwm1, HIGH);
-    digitalWrite(m3pwm2, LOW);
+    //Serial.println("drive 240 deg");
+    move1(-speeeed);
+    move2(speeeed);
+    move3(0);
   }else if (direction > 270 && direction <= 330) {
     //300 degrees
-    digitalWrite(m1pwm1, LOW);
-    digitalWrite(m1pwm2, HIGH);
-    digitalWrite(m2pwm1, LOW);
-    digitalWrite(m2pwm2, HIGH);
-    //Serial.println("drive 300deg");
-    digitalWrite(m3pwm1, LOW);
-    digitalWrite(m3pwm2, LOW);
+    //Serial.println("drive 300 deg");
+    move1(-speeeed);
+    move2(0);
+    move3(speeeed);
   }else{
     Serial.println("how?");  
   }
diff --git a/CODE/debug/driving/base_drive_with_speed_control/base_drive_with_speed_control.ino b/CODE/debug/driving/base_drive_with_speed_control/base_drive_with_speed_control.ino
index 0d992e0..e5a783e 100644
--- a/CODE/debug/driving/base_drive_with_speed_control/base_drive_with_speed_control.ino
+++ b/CODE/debug/driving/base_drive_with_speed_control/base_drive_with_speed_control.ino
@@ -1,13 +1,4 @@
 #include <TimerThree.h>
-#include <Arduino.h>
-
-int speed = 1000; // speed 0-1
-
-volatile int PWMval1 = 0;   // PWM value 0-100
-volatile int PWMval2 = 0;   // PWM value 0-100
-volatile int PWMval3 = 0;   // PWM value 0-100
-
-int led = 13;
 
 int m1enA = 6;
 int m1enB = 5;
@@ -29,7 +20,18 @@ int m3pwm1 = 24;
 int m3pwm2 = 25;
 int m3encoA = 28;
 int m3encoB = 23;
-  
+
+int speed = 1000; // speed 0-1
+
+double sp1 = 0.00;
+double sp2 = 0.00;
+double sp3 = 0.00;
+
+volatile int PWMval1 = 0;   // PWM value 0-100
+volatile int PWMval2 = 0;   // PWM value 0-100
+volatile int PWMval3 = 0;   // PWM value 0-100
+
+
 void setup() {
   pinMode(m1enA, OUTPUT);
   pinMode(m1enB, OUTPUT);
@@ -45,17 +47,14 @@ void setup() {
   pinMode(m3enB, OUTPUT);
   pinMode(m3pwm1, OUTPUT);
   pinMode(m3pwm2, OUTPUT);
-  Serial.begin(9600);
-  Serial.print("start");
-
-  Timer3.initialize(800); // 800 microseconds / 1.25kHz
-  Timer3.attachInterrupt(updatePWM1);
-  Timer3.attachInterrupt(updatePWM2);
-  Timer3.attachInterrupt(updatePWM3);
   
+  Timer3.initialize(800); // 800 microseconds or 1.25kHz
+  Timer3.attachInterrupt(updatePWM); 
+
+  Serial.begin(9600);
 }
 
-void updatePWM1() {
+void updatePWM() {
   static int counter = 0;
 
   if (counter < PWMval1) {
@@ -63,32 +62,14 @@ void updatePWM1() {
   } else {
     digitalWrite(m1enA, LOW);
   }
-
-  counter++;
-  if (counter >= 100) {
-    counter = 0;
-  }
-}
-
-void updatePWM2() {
-  static int counter = 0;
-
-  if (counter < PWMval1) {
+  
+  if (counter < PWMval2) {
     digitalWrite(m2enA, HIGH);
   } else {
     digitalWrite(m2enA, LOW);
   }
-
-  counter++;
-  if (counter >= 100) {
-    counter = 0;
-  }
-}
-
-void updatePWM3() {
-  static int counter = 0;
-
-  if (counter < PWMval1) {
+  
+  if (counter < PWMval3) {
     digitalWrite(m3enA, HIGH);
   } else {
     digitalWrite(m3enA, LOW);
@@ -104,7 +85,7 @@ void mcpsoftpwm1(int value1) {
   if (value1 < 0) value1 = 0;
   if (value1 > 100) value1 = 100;
 
-  Serial.print("softPWM pin: 3, value: ");
+  Serial.print("softPWM pin: enA, value: ");
   Serial.println(value1); 
   PWMval1 = value1; 
 }
@@ -112,110 +93,122 @@ void mcpsoftpwm2(int value2) {
   if (value2 < 0) value2 = 0;
   if (value2 > 100) value2 = 100;
 
-  Serial.print("softPWM pin: 3, value: ");
+  Serial.print("softPWM pin: enA, value: ");
   Serial.println(value2); 
   PWMval2 = value2; 
-}
-void mcpsoftpwm3(int value3) {
+}void mcpsoftpwm3(int value3) {
   if (value3 < 0) value3 = 0;
   if (value3 > 100) value3 = 100;
 
-  Serial.print("softPWM pin: 3, value: ");
+  Serial.print("softPWM pin: enA, value: ");
   Serial.println(value3); 
   PWMval3 = value3; 
 }
-void drive(int speed, int direction){
- //
- // Direction............|.Motor 1.|.Motor 2.|.Motor 3.
- // 0deg(forward)........|....0....|....-....|....+....
- // 60deg................|....+....|....-....|....0....    
- // 120deg...............|....+....|....0....|....-....
- // 180 deg (backward)...|....0....|....+....|....-.... 
- // 240 deg..............|....-....|....+....|....0....
- // 300 deg..............|....-....|....0....|....+....
- //
 
-  mcpsoftpwm1(speed);  //pwm speed signal
-  mcpsoftpwm2(speed);  //pwm speed signal
-  mcpsoftpwm2(speed);  //pwm speed signal
+void move1(int sped1) {
+  sp1 = abs(sped1)*0.1;
+  mcpsoftpwm1(sp1);
   digitalWrite(m1enB, LOW);
-  digitalWrite(m2enB, LOW);
-  digitalWrite(m3enB, LOW);
-  Serial.println("debug2");
-  Serial.println(direction);
-  if(direction>330 || direction<=30){
-    //0 deg
+  if (sped1 < 0) {
+    Serial.print("back ");
     digitalWrite(m1pwm1, LOW);
     digitalWrite(m1pwm2, HIGH);
+  } else {
+    Serial.print("forward ");
+    digitalWrite(m1pwm1, HIGH);
+    digitalWrite(m1pwm2, LOW);
+  } 
+  Serial.println(sp1);
+}
 
+void move2(int sped2) {
+  sp2 = abs(sped2)*0.1;
+  mcpsoftpwm2(sp2);
+  digitalWrite(m2enB, LOW);
+  if (sped2 < 0) {
+    Serial.print("back ");
     digitalWrite(m2pwm1, LOW);
+    digitalWrite(m2pwm2, HIGH);
+  } else {
+    Serial.print("forward ");
+    digitalWrite(m2pwm1, HIGH);
     digitalWrite(m2pwm2, LOW);
+  } 
+  Serial.println(sp2);
+}
 
+void move3(int sped3) {
+  sp3 = abs(sped3)*0.1;
+  mcpsoftpwm3(sp3);
+  digitalWrite(m3enB, LOW);
+  if (sped3 < 0) {
+    Serial.print("back ");
     digitalWrite(m3pwm1, LOW);
     digitalWrite(m3pwm2, HIGH);
-    Serial.println("0 deg"); 
+  } else {
+    Serial.print("forward ");
+    digitalWrite(m3pwm1, HIGH);
+    digitalWrite(m3pwm2, LOW);
+  } 
+  Serial.println(sp3);
+}
+
+void drive(int speeeed, int direction){
+ //
+ // Direction............|.Motor 1.|.Motor 2.|.Motor 3.
+ // 0deg(forward)........|....0....|....-....|....+....
+ // 60deg................|....+....|....-....|....0....    
+ // 120deg...............|....+....|....0....|....-....
+ // 180 deg (backward)...|....0....|....+....|....-.... 
+ // 240 deg..............|....-....|....+....|....0....
+ // 300 deg..............|....-....|....0....|....+....
+ //
+ if(direction>330 || direction<=30){
+    //0 deg
+    //Serial.println("drive 0 deg"); 
+    move1(0);
+    move2(-speeeed);
+    move3(speeeed);
   }else if (direction>30 && direction <= 90){
     //60 deg
-    Serial.println("60deg");
-    digitalWrite(m1pwm1, LOW);
-    digitalWrite(m1pwm2, LOW);
-
-    digitalWrite(m2pwm1, HIGH);
-    digitalWrite(m2pwm2, LOW);
-    
-    digitalWrite(m3pwm1, LOW);
-    digitalWrite(m3pwm2, HIGH);
+    //Serial.println("drive 60deg");
+    move1(speeeed);
+    move2(-speeeed);
+    move3(0);
   }else if (direction>90 && direction <= 150){
     //120 deg
-    Serial.println("120 deg");
-    digitalWrite(m1pwm1, HIGH);
-    digitalWrite(m1pwm2, LOW);
-    digitalWrite(m2pwm1, HIGH);
-    digitalWrite(m2pwm2, LOW);
-    digitalWrite(m3pwm1, LOW);
-    digitalWrite(m3pwm2, LOW);
+    //Serial.println("drive 120 deg");
+    move1(speeeed);
+    move2(0);
+    move3(-speeeed);
   }else if (direction > 150 && direction <= 210) {
     //180 degrees
-    Serial.println("on 180");
-    digitalWrite(m1pwm1, HIGH);
-    digitalWrite(m1pwm2, LOW);
-
-    digitalWrite(m2pwm1, LOW);
-    digitalWrite(m2pwm2, LOW);
-    
-    digitalWrite(m3pwm1, HIGH);
-    digitalWrite(m3pwm2, LOW);
+    //Serial.println("drive  180 deg");
+    move1(0);
+    move2(speeeed);
+    move3(-speeeed);
   }else if (direction > 210 && direction <= 270) {
     //240 degrees
-    digitalWrite(m1pwm1, LOW);
-    digitalWrite(m1pwm2, LOW);
-
-    digitalWrite(m2pwm1, LOW);
-    digitalWrite(m2pwm2, HIGH);
-
-    digitalWrite(m3pwm1, HIGH);
-    digitalWrite(m3pwm2, LOW);
+    //Serial.println("drive 240 deg");
+    move1(-speeeed);
+    move2(speeeed);
+    move3(0);
   }else if (direction > 270 && direction <= 330) {
     //300 degrees
-    digitalWrite(m1pwm1, LOW);
-    digitalWrite(m1pwm2, HIGH);
-    digitalWrite(m2pwm1, LOW);
-    digitalWrite(m2pwm2, HIGH);
-    Serial.println("300deg");
-    digitalWrite(m3pwm1, LOW);
-    digitalWrite(m3pwm2, LOW);
+    //Serial.println("drive 300 deg");
+    move1(-speeeed);
+    move2(0);
+    move3(speeeed);
   }else{
     Serial.println("how?");  
   }
 }
 
-// the loop routine runs over and over again forever:
 void loop() {
-  drive(255, 0);
+  drive(1000, 0);
   Serial.println("ffor");
   delay(1500);
-  drive(255, 180);
+  drive(500, 180);
   Serial.println("back");
-  delay(1500);
-
+  delay(3000);
 }
diff --git a/CODE/debug/driving/onemotor_softPWM/onemotor_softPWM.ino b/CODE/debug/driving/onemotor_softPWM/onemotor_softPWM.ino
index 2f57d59..68d95dd 100644
--- a/CODE/debug/driving/onemotor_softPWM/onemotor_softPWM.ino
+++ b/CODE/debug/driving/onemotor_softPWM/onemotor_softPWM.ino
@@ -1,11 +1,11 @@
 #include <TimerThree.h>
 
-int enA = 1;  //PWM pin
-int enB = 2;  //GND
-int pwm1 = 3; //
-int pwm2 = 4;
-int encoA = 5;
-int encoB = 6;
+int enA = 6;  //PWM pin
+int enB = 5;  //GND
+int pwm1 = 4; //
+int pwm2 = 3;
+int encoA = 32;
+int encoB = 31;
 
 int speed = 1000; // speed 0-1