Diff: actuators.cpp

Revision:

115:d8d2968981f3

Parent:

110:a6439e13be8b

Child:

116:8b812e268b85

--- a/actuators.cpp	Tue Oct 27 14:45:15 2015 +0000
+++ b/actuators.cpp	Tue Oct 27 16:21:20 2015 +0100
@@ -54,8 +54,8 @@
 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 motorCalSpeed = 5; // deg/sec
+double returnSpeed = -5;
 bool springHit = false;
 float lastCall = 0;
 bool calibrating1 = true;
@@ -115,13 +115,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
@@ -191,20 +189,21 @@
    safetyOn = false; // safety springs off
    motorsEnable = true; // motors on
    redLed.write(0); greenLed.write(1); blueLed.write(1);
-   while (calibrating1 || calibrating2){
+   if (calibrating1 || calibrating2){
        if (calibrating1){
             motor1SetSpeed = motorCalSpeed;
             redLed.write(1); greenLed.write(0); blueLed.write(1);
            if(safetyIn.read() !=1){ // check if arm reached safety position
                encoder1.setPosition(0); // set motor 1 cal angle
+               motor1PrevCounts = 0; // set previous count to prevent speed spike
                motor1SetSpeed = returnSpeed; // move away
                springHit = true;
            }else{ 
                if(springHit){ // if hit and after is no longer touching spring
                 motor1SetSpeed = 0;
                 springHit = false;
+                calibrating2 = true; // start calibrating 2
                 calibrating1 = false;
-                calibrating2 = true; // start calibrating 2
                }
            }
        }
@@ -213,6 +212,7 @@
             redLed.write(1); greenLed.write(1); blueLed.write(0);
            if(safetyIn.read() !=1){ // check if arm reached safety position
                encoder2.setPosition(0); // set motor 2 cal angle
+               motor2PrevCounts = 0; // set previous cunt to prevent speed spike
                motor2SetSpeed = returnSpeed; // move away
                springHit = true;
            }else{ 
@@ -224,10 +224,11 @@
            }
        }
        now = t.read(); // call motor using timer instead of wait
-       if(now - lastCall > motorCall){
+       // if(now - lastCall > motorCall){
            motorControl();
-           lastCall = now;
-       }
+           wait(motorCall);
+           // lastCall = now;
+       // }
 
    }
     motorsEnable = false; // turn motor's off again