Diff: actuators.cpp

Revision:

116:8b812e268b85

Parent:

115:d8d2968981f3

Child:

117:b1667291748d

--- a/actuators.cpp	Tue Oct 27 16:21:20 2015 +0100
+++ b/actuators.cpp	Wed Oct 28 11:48:07 2015 +0100
@@ -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 = 5; // deg/sec
-double returnSpeed = -5;
+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
@@ -116,10 +117,10 @@
 
 void motorControl(){
         // EMG signals to motor speeds
-//        const double scaleVel = 20;
-//        motor1SetSpeed = x_velocity*scaleVel;
-//        motor2SetSpeed = y_velocity*scaleVel;
-//        servoSpeed = z_velocity*scaleVel;
+    //  const double scaleVel = 20;
+    //  motor1SetSpeed = x_velocity*scaleVel;
+    //  motor2SetSpeed = y_velocity*scaleVel;
+    //  servoSpeed = z_velocity*scaleVel;
     // get encoder positions in degrees
         // 131.25:1 gear ratio
         // getPosition uses X2 configuration, so 32 counts per revolution
@@ -129,22 +130,26 @@
         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;
         
     // calculate motor setpoint speed in deg/sec from setpoint x/y speed
 
-        
+    // exclude kinematics when still calibrating
+    if (calReady){
+      kinematics();
+    }
+
     if(motorsEnable){  // only run motors if switch is enabled
     // compute new PID parameters using setpoint angle speeds and encoder speed
         writeMotors();
@@ -161,17 +166,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);
@@ -185,21 +190,25 @@
     servo.SetPosition(1300 + 700*servoSpeed);
 }
 
+const double motor1StartPos = (-10*32*131.25)/360; // angle to encoder counts
+const double motor2StartPos = (114*32*131.25)/360; 
+
 bool calibrateMotors(){
    safetyOn = false; // safety springs off
    motorsEnable = true; // motors on
-   redLed.write(0); greenLed.write(1); blueLed.write(1);
-   if (calibrating1 || calibrating2){
+   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
-               encoder1.setPosition(0); // set motor 1 cal angle
-               motor1PrevCounts = 0; // set previous count to prevent speed spike
-               motor1SetSpeed = returnSpeed; // move away
+           if(safetyIn.read() !=1 && !springHit){ // check if arm reached safety position
+               encoder1.setPosition(motor1StartPos); // set motor 1 cal angle
+               motor1PrevCounts = motor1StartPos; // 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
@@ -208,29 +217,31 @@
            }
        }
        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
-               encoder2.setPosition(0); // set motor 2 cal angle
-               motor2PrevCounts = 0; // set previous cunt to prevent speed spike
-               motor2SetSpeed = returnSpeed; // move away
+           if(safetyIn.read() !=1 && !springHit){ // check if arm reached safety position
+               encoder2.setPosition(motor2StartPos); // set motor 2 cal angle
+               motor2PrevCounts = motor2StartPos; // 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
+       // now = t.read(); // call motor using timer instead of wait
        // if(now - lastCall > motorCall){
            motorControl();
-           wait(motorCall);
+           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
@@ -245,31 +256,40 @@
     }
 }
 
+const double L1 = 0.436; // first arm in m
+const double L2 = 0.120; // first arm in m
+const double L3 = 0.255; // servo arm in m
+const double Xmax = 0.3;
+const double Xmin; = -0.3;
+const double Ymax = 0.645;
+const double Ymin = 0.33;
 
-//bool kinematics(){
-//     // calculate current x and Y
-//     X = L2*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180);
-//     Y = L2*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180);
-//     // check if x and y are within limits
-//     //  else  Store the constraint line
-//     //      check if movement is in direction of constraint
-//     //      else return false no movement (anglespeed = 0)
-//     // calculate required angle speeds
-//     if( (X>Xmax && setXSpeed > 0 )|| \
-//         (X<Xmin && setXSpeed < 0 )|| \
-//         (Y>Ymax && setYSpeed > 0 )|| \
-//         (Y<Ymin && setYSpeed < 0 )   \
-//     ){
-//         motor1SetSpeed = 0;
-//         motor2SetSpeed = 0;
-//         return false;
-//         break;
-//     }
-// motor1SetSpeed = (setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \
-//     setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180));
-// motor2SetSpeed = -(setXSpeed*L2*cos((motor1Pos + motor2Pos)*PI/180) + \
-//     setYSpeed*L2*sin((motor1Pos + motor2Pos)*PI/180) + \
-//     setXSpeed*L1*cos(motor1Pos*PI/180) + \
-//     setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*L2*sin(motor2Pos*PI/180));
+
 
-//}
\ No newline at end of file
+bool kinematics(){
+    // calculate current x and Y
+    double X = (L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180);
+    double Y = (L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180);
+    // check if x and y are within limits
+    //  else  Store the constraint line
+    //      check if movement is in direction of constraint
+    //      else return false no movement (anglespeed = 0)
+    // calculate required angle speeds
+    if( (X>Xmax && setXSpeed > 0 )|| \
+        (X<Xmin && setXSpeed < 0 )|| \
+        (Y>Ymax && setYSpeed > 0 )|| \
+        (Y<Ymin && setYSpeed < 0 )   \
+    ){
+        motor1SetSpeed = 0;
+        motor2SetSpeed = 0;
+        return false;
+        break;
+    }
+motor1SetSpeed = (setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \
+    setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180));
+motor2SetSpeed = -(setXSpeed*(L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + \
+    setYSpeed*(L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + \
+    setXSpeed*L1*cos(motor1Pos*PI/180) + \
+    setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*(L2+L3)*sin(motor2Pos*PI/180));
+
+}
\ No newline at end of file