Revision 111:43c0881fe7e7, committed 2015-10-27

Comitter:

annesteenbeek

Date:

Tue Oct 27 17:10:33 2015 +0000

Parent:

110:a6439e13be8b

Child:

112:7b964afb97b4

Commit message:

working calibration and correct directions

Changed in this revision

--- 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

--- a/main.cpp	Tue Oct 27 14:45:15 2015 +0000
+++ b/main.cpp	Tue Oct 27 17:10:33 2015 +0000
@@ -14,8 +14,6 @@
 #include "debug.h"
 #include "emg.h"
 
-int a =4;
-
 Ticker switchesTick, debugTick, motorTick, EMGTick, safetyTick;
 volatile bool switches_go=false, debug_go=false, motor_go=false, emg_go=false, safety_go=false;
 
@@ -25,23 +23,32 @@
 void emg_activate(){emg_go=true;};
 void safety_activate(){safety_go=true;};
 
-double motorCall = 0.01; // set motor frequency global so it can be used for speed.
-const int sample = 0;  // Constant for mode switching for program readability
-const int normalize = 1; // Constant for mode switching for program readability
-bool mode = normalize; // Set program mode
-bool calReady = false; // flag for calibration ready
-
-int main(){
-    motorInit();    
-    calReady = calibrateMotors(); // start motor calibration
-
+void tickerAttach(){
     EMGTick.attach(&emg_activate, 0.005f);
     switchesTick.attach(&switches_activate, 0.02f);
     debugTick.attach(&debug_activate, 0.03f);
     motorTick.attach(&motor_activate, motorCall);
     safetyTick.attach(&safety_activate, 0.001f);
+}
+
+double motorCall = 0.01; // set motor frequency global so it can be used for speed.
+const int sample = 0;  // Constant for mode switching for program readability
+const int normalize = 1; // Constant for mode switching for program readability
+bool mode = normalize; // Set program mode
+bool tickersActivated = false;
+bool calReady = false; // flag for calibration ready
+
+int main(){
+    motorInit();
+    calReady = calibrateMotors();
 
     while (true) {
+
+        if(calReady && !tickersActivated){ // when done with calibration, start tickers
+            tickerAttach();
+            tickersActivated = true;
+        }
+
         if(emg_go){
             emg_go=false;
             readEMG();