Diff: actuators.cpp

Revision:

30:a20f16bf8dda

Parent:

29:e4f3455aaa0b

Child:

31:8fbee6c92753

--- a/actuators.cpp	Tue Oct 06 17:55:11 2015 +0000
+++ b/actuators.cpp	Tue Oct 06 21:06:20 2015 +0000
@@ -25,12 +25,12 @@
 
 // Set PID values
 float Kp1 = 1; 
-float Ki1 = 1; 
-float Kd1 = 1;
+float Ki1 = 0; 
+float Kd1 = 0;
 
 float Kp2 = 1; 
-float Ki2 = 1; 
-float Kd2 = 1;
+float Ki2 = 0; 
+float Kd2 = 0;
 
 float PIDinterval = 0.2;
 
@@ -39,6 +39,7 @@
 float prevTime = 0;
 float curTime = 0;
 
+float pidError = 0;
 // Create object instances
 // Initialze motors
 PwmOut motor1(motor1PWMPin);
@@ -71,13 +72,18 @@
 
     PIDmotor1.setProcessValue(motorSpeed1);
     PIDmotor2.setProcessValue(motorSpeed2);
-    // set PID mode
+
+    // set limits for PID output to avoid integrator build up.
+    PIDmotor1.setInputLimits(-100, 300);
+    PIDmotor2.setInputLimits(-100,300);
+    PIDmotor1.setOutputLimits(-100, 300);
+    PIDmotor2.setOutputLimits(-100, 300);
+    
+    // Turn PID on
     PIDmotor1.setMode(1);
     PIDmotor2.setMode(1);
-
-    // set limits for PID output to avoid integrator build up.
-    PIDmotor1.setOutputLimits(-1.0, 1.0);
-    PIDmotor2.setOutputLimits(-1.0, 1.0);
+    
+    // start the timer
     t.start();
 }
 
@@ -85,13 +91,16 @@
 void motorControl(){
     if(motorEnable){  // only run motors if switch is enabled
 
-    // get encoder positions
-        motor1Pos = encoder1.getPosition();
-        motor2Pos = encoder2.getPosition();
+    // get encoder positions in degrees
+        // 131.25:1 gear ratio
+        // getPosition uses X2 configuration, so 32 counts per revolution
+        // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive
+        motor1Pos = -((encoder1.getPosition()/32)/131.25)*360;
+        motor2Pos = -((encoder2.getPosition()/32)/131.25)*360;
 
         // check if motor's are within rotational boundarys
-    // get  encoder speeds
-        curTime = t.read()
+    // get  encoder speeds in deg/sec
+        curTime = t.read();
         motorSpeed1 = (motor1Pos - prevMotor1Pos)/(curTime-prevTime);
         motorSpeed2 = (motor2Pos - prevMotor2Pos)/(curTime-prevTime);
         prevTime = curTime;
@@ -101,23 +110,33 @@
         
     // translate to x/y speed
     // compute new PID parameters using setpoint speeds and x/y speeds
+        PIDmotor1.setSetPoint(motorSetSpeed1);
+        PIDmotor2.setSetPoint(motorSetSpeed2);
+
+        PIDmotor1.setProcessValue(motorSpeed1);
+        PIDmotor2.setProcessValue(motorSpeed2);
+    
         motorPWM1 = PIDmotor1.compute();
         motorPWM2 = PIDmotor2.compute();
     // translate to motor rotation speed
     // write new values to motor's
-        if (motorPWM1 > 0 ){ // CCW rotation (unitcircle convetion)
+        if (motorPWM1 > 0 ){ // CCW rotation 
             direction1 = false;
         }else{
             direction1 = true; // CW rotation
         }
-        if (motorPWM2 > 0 ){ // CCW rotation (unitcircle convetion)
+        if (motorPWM2 > 0 ){ // CCW rotation 
             direction2 = false;
         }else{
             direction2 = true; // CW rotation
         }
-        motor1.write(abs(motorPWM1));
-        // motor2.write(abs(motorPWM2));
-        motor2.write(motorSetSpeed2);
+        motor1Dir.write(direction1);
+        motor2Dir.write(direction2);
+        
+        motor1.write(abs(motorPWM1)/300);
+        motor2.write(abs(motorPWM2)/300);
+//        motor2.write(motorSetSpeed2);
+        pidError = PIDmotor2.getError();
 
     }else{
         // write 0 to motors