fix PWM

1 files changed, 92 insertions, 94 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?");  
   }