Diff: main.cpp

Revision:

28:8cd898ff43a2

Parent:

27:a4228ea8fb8f

Child:

29:9aa4d63a9bd1

--- a/main.cpp	Fri Oct 13 11:59:15 2017 +0000
+++ b/main.cpp	Fri Oct 13 16:24:50 2017 +0000
@@ -8,7 +8,7 @@
 const float gearRatio = 131;
 
 // Controller parameters
-const float k_p = 0.5;
+const float k_p = 1;
 const float k_i = 0; // Still needs a reasonable value
 const float k_d = 0; // Again, still need to pick a reasonable value
 
@@ -33,7 +33,7 @@
 InterruptIn sw2(SW2);
 InterruptIn sw3(SW3);
 InterruptIn button1(D2);
-InterruptIn button2(D4);
+InterruptIn button2(D3);
 AnalogIn pot1(A0);
 AnalogIn pot2(A1);
 
@@ -118,15 +118,7 @@
 // motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
 float motorController(float e_pos, float e_int, float e_der){
     float motorValue;
-    if(e_pos >= 0){
-        // Positive error yields positive motor value = counterclockwise rotation
-        motorValue = -1*((e_pos*k_p) + k_i*e_int + k_d*e_der + 0.45);
-        }    
-    else {
-        // Negative error yields negative motor value = clockwise rotation
-        motorValue = 1*((e_pos*k_p) + k_i*e_int + k_d*e_der + 0.45);
-     }
-     
+    motorValue = k_p*e_pos + k_d*e_der + k_i*e_int;
     return motorValue;
     }
     
@@ -135,18 +127,32 @@
     switch(currentState){
                 case KILLED:
                     motor1_pwm.write(0.0);
+                    // Set motor direction
+                    if (motorValue >=0){
+                        pc.printf("Fa! \r\n");
+                        motor1_direction = 0; //This doesn't seem to set motor 1 direction to 0
+                        pc.printf("M1D = %d \r\n", motor1_direction.read());
+                        pc.printf("Foo! \r\n");
+                        } else if(motorValue < 0){
+                        motor1_direction = 1;
+                        pc.printf("Bah!");
+                        }
                     ledR = 0;
                     ledG = 1;
                     ledB = 1;
                     break;
                 case ACTIVE:
+                    pc.printf("Got into ACTIVE \r\n");
                     // Set motor direction
                     if (motorValue >=0){
-                        motor1_direction = 0;
+                        pc.printf("Fa!");
+                        motor1_direction.write(0);
+                        pc.printf("Foo!");
+                        } else if(motorValue < 0){
+                        motor1_direction.write(1);
+                        pc.printf("Bah!");
                         }
-                    else {
-                        motor1_direction = 1;
-                        }
+                        
                     // Set motor speed
                     if (fabs(motorValue)>1){ 
                         motor1_pwm = 1;
@@ -166,6 +172,10 @@
     float motorValue = motorController(e_pos, e_int, e_der);
     float r = getReferencePosition();
     sendDataToPc(r, e_pos, e_int, e_der, motorValue);
+    pc.printf("Motorvalue is %.2f \r\n", motorValue);
+    pc.printf("Error is %.2f \r\n", e_pos);
+    pc.printf("Reference is %.2f \r\n", r);
+    pc.printf("motor1 direction is %d \r\n", motor1_direction.read());
     setMotor1(motorValue);
     }