Diff: src/motor.cpp

Revision:

45:0db0fc9f77b1

Parent:

44:3566c5699ba6

Child:

49:3aaa790800ad

diff -r 3566c5699ba6 -r 0db0fc9f77b1 src/motor.cpp
--- a/src/motor.cpp	Wed Nov 27 02:51:39 2013 +0000
+++ b/src/motor.cpp	Sun Dec 01 07:23:22 2013 +0000
@@ -40,20 +40,20 @@
     
     float dCurrent=0;
     switch(mode){
-        default:{ //probably will work now, previously this was on the bottom and so overrode dCurrent
+        default:{ 
             //dCurrent = kp*(dAngle-angle)+kd*(dAngularVelocity-speed);
             dCurrent = dTorque/3.27;
             break;
             }
-        case 1:{
+        case 1:{//Positional Control
             dCurrent = kp*(dAngle-angle);
             break;
             }
-        case 2:{
-            dCurrent = kd*(dAngularVelocity-speed);
+        case 2:{//Velocity Control
+            dCurrent = kp*(dAngularVelocity-speed);
             break;
             }
-        case 3:{
+        case 3:{  //Torque Control
             dCurrent = dTorque/3.27;
             break;
             }
@@ -76,8 +76,6 @@
     }
     pwmOut.period_us(100);
     pwmOut.write(duty);
-    
-
 }
 
 float Motor::getCurrent()
@@ -100,7 +98,7 @@
 }
 
 void Motor::setPos(float pos){
-    mode = -1;
+    mode = 1;
     dAngle=pos;
     dAngularVelocity = 0;
 }