Diff: actuators.cpp

Revision:

112:7b964afb97b4

Parent:

111:43c0881fe7e7

Child:

117:b1667291748d

--- a/actuators.cpp	Tue Oct 27 17:10:33 2015 +0000
+++ b/actuators.cpp	Wed Oct 28 10:47:04 2015 +0000
@@ -62,6 +62,8 @@
 bool calibrating2 = false;
 double looseTime = 0;
 
+bool calReady = false;
+
 // Create object instances
 // Safety Pin
 DigitalIn safetyIn(safetyPin);
@@ -117,10 +119,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
@@ -145,7 +147,11 @@
         
     // 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();
@@ -186,6 +192,9 @@
     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
@@ -194,8 +203,8 @@
             motor1SetSpeed = motor1CalSpeed;
             redLed.write(1); greenLed.write(0); blueLed.write(1);
            if(safetyIn.read() !=1 && !springHit){ // check if arm reached safety position
-               encoder1.setPosition(0); // set motor 1 cal angle
-               motor1PrevCounts = 0; // set previous count to prevent speed spike
+               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
@@ -213,8 +222,8 @@
             motor2SetSpeed = motor2CalSpeed; 
             redLed.write(1); greenLed.write(1); blueLed.write(0);
            if(safetyIn.read() !=1 && !springHit){ // check if arm reached safety position
-               encoder2.setPosition(0); // set motor 2 cal angle
-               motor2PrevCounts = 0; // set previous cunt to prevent speed spike
+               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();
@@ -249,31 +258,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