Diff: src/motor.cpp

Revision:

44:3566c5699ba6

Parent:

43:68faf056ed5c

Child:

45:0db0fc9f77b1

diff -r 68faf056ed5c -r 3566c5699ba6 src/motor.cpp
--- a/src/motor.cpp	Tue Nov 26 21:26:44 2013 +0000
+++ b/src/motor.cpp	Wed Nov 27 02:51:39 2013 +0000
@@ -8,6 +8,8 @@
     voltage = 12.0;
     mode=0;
     
+    pos=0;
+    
     kp=3;
     kd=0.04;
     
@@ -36,16 +38,25 @@
     float mCurrent = getCurrent();
     //pc.printf("current: %f  speed: %f\n", mCurrent, speed);
     
-    float dCurrent;
+    float dCurrent=0;
     switch(mode){
-        case 1:
+        default:{ //probably will work now, previously this was on the bottom and so overrode dCurrent
+            //dCurrent = kp*(dAngle-angle)+kd*(dAngularVelocity-speed);
+            dCurrent = dTorque/3.27;
+            break;
+            }
+        case 1:{
             dCurrent = kp*(dAngle-angle);
-        case 2:
-            dCurrent = kd*10*(dAngularVelocity-speed);
-        case 3:
+            break;
+            }
+        case 2:{
+            dCurrent = kd*(dAngularVelocity-speed);
+            break;
+            }
+        case 3:{
             dCurrent = dTorque/3.27;
-        default:
-            dCurrent = kp*(dAngle-angle)+kd*(dAngularVelocity-speed);
+            break;
+            }
     }