Diff: actuators.cpp

Revision:

111:43c0881fe7e7

Parent:

110:a6439e13be8b

Child:

112:7b964afb97b4

--- a/actuators.cpp	Tue Oct 27 14:45:15 2015 +0000
+++ b/actuators.cpp	Tue Oct 27 17:10:33 2015 +0000
@@ -54,12 +54,13 @@
 const int servoStartPos = 1300; // Servo ranges from 600ms(-90) to 2000ms(90), 1300 is 0 deg
 
 // Set calibration values
-double motorCalSpeed = 10; // deg/sec
-double returnSpeed = -10;
+double motor1CalSpeed = -5; // deg/sec
+double motor2CalSpeed = 5;
 bool springHit = false;
 float lastCall = 0;
 bool calibrating1 = true;
 bool calibrating2 = false;
+double looseTime = 0;
 
 // Create object instances
 // Safety Pin
@@ -115,13 +116,11 @@
 
 
 void motorControl(){
-    #ifndef SETPOS // if not tuning with potmeters, switch to EMG
         // EMG signals to motor speeds
 //        const double scaleVel = 20;
 //        motor1SetSpeed = x_velocity*scaleVel;
 //        motor2SetSpeed = y_velocity*scaleVel;
 //        servoSpeed = z_velocity*scaleVel;
-    #endif
     // get encoder positions in degrees
         // 131.25:1 gear ratio
         // getPosition uses X2 configuration, so 32 counts per revolution
@@ -131,15 +130,15 @@
         encoder2Counts = encoder2.getPosition();
 
 
-        motor1Pos = -((encoder1Counts/32)/131.25)*360;
-        motor2Pos = -((encoder2Counts/32)/131.25)*360;
+        motor1Pos = ((encoder1Counts/32)/131.25)*360;
+        motor2Pos = ((encoder2Counts/32)/131.25)*360;
 
         // check if motor's are within rotational boundarys
     // get  encoder speeds in deg/sec
         now = t.read(); 
         timechange = (now - prevTime);
-        motor1Speed = -((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange;
-        motor2Speed = -((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange;
+        motor1Speed = ((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange;
+        motor2Speed = ((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange;
         prevTime = now;
         motor1PrevCounts = encoder1Counts;
         motor2PrevCounts = encoder2Counts;
@@ -163,17 +162,17 @@
     motor2PID.Compute();
 // write new values to motor's
     if (motor1SetSpeed > 0 ){ // CCW rotation 
-        direction1 = false;
+        direction1 = true;
         motor1PID.SetOutputLimits(0,1); // change pid output direction
     }else{
-        direction1 = true; // CW rotation
+        direction1 = false; // CW rotation
         motor1PID.SetOutputLimits(-1,0);
     }
     if (motor2SetSpeed > 0 ){ // CCW rotation 
-        direction2 = false;
+        direction2 = true;
         motor2PID.SetOutputLimits(0,1);
     }else{
-        direction2 = true; // CW rotation
+        direction2 = false; // CW rotation
         motor2PID.SetOutputLimits(-1,0);
     }
     motor1Dir.write(direction1);
@@ -190,46 +189,52 @@
 bool calibrateMotors(){
    safetyOn = false; // safety springs off
    motorsEnable = true; // motors on
-   redLed.write(0); greenLed.write(1); blueLed.write(1);
    while (calibrating1 || calibrating2){
        if (calibrating1){
-            motor1SetSpeed = motorCalSpeed;
+            motor1SetSpeed = motor1CalSpeed;
             redLed.write(1); greenLed.write(0); blueLed.write(1);
-           if(safetyIn.read() !=1){ // check if arm reached safety position
+           if(safetyIn.read() !=1 && !springHit){ // check if arm reached safety position
                encoder1.setPosition(0); // set motor 1 cal angle
-               motor1SetSpeed = returnSpeed; // move away
+               motor1PrevCounts = 0; // set previous count to prevent speed spike
+               motor1CalSpeed = -motor1CalSpeed; // move away
                springHit = true;
+               looseTime = t.read(); // timer to compensate spring movement
            }else{ 
-               if(springHit){ // if hit and after is no longer touching spring
+              // if hit and after is no longer touching spring and 0.5seconds passed
+               if(springHit && ((t.read() - looseTime) > 2)){ 
                 motor1SetSpeed = 0;
                 springHit = false;
+                calibrating2 = true; // start calibrating 2
                 calibrating1 = false;
-                calibrating2 = true; // start calibrating 2
                }
            }
        }
        if (calibrating2){
-            motor2SetSpeed = motorCalSpeed; 
+            motor2SetSpeed = motor2CalSpeed; 
             redLed.write(1); greenLed.write(1); blueLed.write(0);
-           if(safetyIn.read() !=1){ // check if arm reached safety position
+           if(safetyIn.read() !=1 && !springHit){ // check if arm reached safety position
                encoder2.setPosition(0); // set motor 2 cal angle
-               motor2SetSpeed = returnSpeed; // move away
+               motor2PrevCounts = 0; // set previous cunt to prevent speed spike
+               motor2CalSpeed = -motor2CalSpeed; // move away
                springHit = true;
+               looseTime = t.read();
            }else{ 
-               if(springHit){ // if hit and after is no longer touching spring
+               if(springHit && ((t.read() - looseTime) > 2)){ 
                 motor2SetSpeed = 0;
                 springHit = false;
                 calibrating2 = false; // stop calibrating 2
                }
            }
        }
-       now = t.read(); // call motor using timer instead of wait
-       if(now - lastCall > motorCall){
+       // now = t.read(); // call motor using timer instead of wait
+       // if(now - lastCall > motorCall){
            motorControl();
-           lastCall = now;
-       }
+           wait(motorCall); // keep calling PID's with motorCall frequency
+           // lastCall = now;
+       // }
 
    }
+    redLed.write(0); greenLed.write(1); blueLed.write(1);
     motorsEnable = false; // turn motor's off again
     safetyOn = true; // turn safety on after callibration
     return true; // return true when finished