added encoder again

Multi-line description of commit,
feel free to be detailed.

Signed-off-by: Abel Tim <git@abeltim.com>

1 files changed, 34 insertions, 19 deletions

diff --git a/code/debug/motors/3motor_PWM/src/main.cpp b/code/debug/motors/3motor_PWM/src/main.cpp
index c9753d5..1a0fa98 100644
--- a/code/debug/motors/3motor_PWM/src/main.cpp
+++ b/code/debug/motors/3motor_PWM/src/main.cpp
@@ -120,13 +120,25 @@ void updatePWM() {
 
 void drive(int speed, int direction){
  //
+ //!!!
+ // If the motors are spinning incorectly, change pins not the drive function.
+ //!!!
+ //
+ // 0 = motor off
+ // + = motor spinning forwards
+ // - = motor spinning backwards
+ //
+ // Motor 1 is the left one.
+ // Motor 2 is the one in the back.
+ // Motor 3 is the right one.
+ //
  // Direction............|.Motor 1.|.Motor 2.|.Motor 3.
- // 0deg(forward)........|....0....|....-....|....+....
- // 60deg................|....+....|....-....|....0....    
- // 120deg...............|....+....|....0....|....-....
- // 180 deg (backward)...|....0....|....+....|....-.... 
- // 240 deg..............|....-....|....+....|....0....
- // 300 deg..............|....-....|....0....|....+....
+ // 0deg(forward)........|....-....|....0....|....+....
+ // 60deg................|....0....|....-....|....+....    
+ // 120deg...............|....+....|....-....|....0....
+ // 180 deg (backward)...|....+....|....0....|....-.... 
+ // 240 deg..............|....0....|....+....|....-....
+ // 300 deg..............|....-....|....+....|....0....
  //
 
   PWMval1 = speed;
@@ -139,15 +151,15 @@ void drive(int speed, int direction){
   Serial.println(direction);
   if(direction>330 || direction<=30){
     //0 deg
-    digitalWrite(m1pwm1, HIGH);
-    digitalWrite(m1pwm2, LOW);
+    Serial.println("0 deg");
+    digitalWrite(m1pwm1, LOW);
+    digitalWrite(m1pwm2, HIGH);
 
     digitalWrite(m2pwm1, LOW);
     digitalWrite(m2pwm2, LOW);
 
     digitalWrite(m3pwm1, HIGH);
-    digitalWrite(m3pwm2, LOW);
-    Serial.println("0 deg"); 
+    digitalWrite(m3pwm2, LOW); 
   }else if (direction>30 && direction <= 90){
     //60 deg
     Serial.println("60deg");
@@ -157,22 +169,24 @@ void drive(int speed, int direction){
     digitalWrite(m2pwm1, LOW);
     digitalWrite(m2pwm2, HIGH);
     
-    digitalWrite(m3pwm1, LOW);
-    digitalWrite(m3pwm2, HIGH);
+    digitalWrite(m3pwm1, HIGH);
+    digitalWrite(m3pwm2, LOW);
   }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(m2pwm1, LOW);
+    digitalWrite(m2pwm2, HIGH);
+
     digitalWrite(m3pwm1, LOW);
     digitalWrite(m3pwm2, LOW);
   }else if (direction > 150 && direction <= 210) {
     //180 degrees
     Serial.println("on 180");
-    digitalWrite(m1pwm1, LOW);
-    digitalWrite(m1pwm2, HIGH);
+    digitalWrite(m1pwm1, HIGH);
+    digitalWrite(m1pwm2, LOW);
 
     digitalWrite(m2pwm1, LOW);
     digitalWrite(m2pwm2, LOW);
@@ -181,6 +195,7 @@ void drive(int speed, int direction){
     digitalWrite(m3pwm2, HIGH);
   }else if (direction > 210 && direction <= 270) {
     //240 degrees
+    Serial.println("240deg");
     digitalWrite(m1pwm1, LOW);
     digitalWrite(m1pwm2, LOW);
 
@@ -191,13 +206,13 @@ void drive(int speed, int direction){
     digitalWrite(m3pwm2, HIGH);
   }else if (direction > 270 && direction <= 330) {
     //300 degrees
-    digitalWrite(m1pwm1, HIGH);
-    digitalWrite(m1pwm2, LOW);
+    Serial.println("300deg");
+    digitalWrite(m1pwm1, LOW);
+    digitalWrite(m1pwm2, HIGH);
 
     digitalWrite(m2pwm1, HIGH);
     digitalWrite(m2pwm2, LOW);
 
-    Serial.println("300deg");
     digitalWrite(m3pwm1, LOW);
     digitalWrite(m3pwm2, LOW);
   }else{