Diff: actuators.cpp

Revision:

127:831f03471efb

Parent:

124:f67ce69557db

--- a/actuators.cpp	Thu Oct 29 20:06:41 2015 +0000
+++ b/actuators.cpp	Thu Oct 29 22:46:30 2015 +0100
@@ -80,6 +80,10 @@
 PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
 PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
 
+// set pumpswitch
+DigitalOut pumpSwitch(pumpPin);
+
+
 Timer t;  
     
 void motorInit(){
@@ -110,7 +114,7 @@
 
 
 void motorControl(){
-        // EMG signals to motor speeds
+  // EMG signals to motor speeds
   if(!usePotmeters && controlAngle){
       double scaleVel = 20;
       motor1SetSpeed = x_velocity*scaleVel;
@@ -165,7 +169,7 @@
 void writeMotors(){
     motor1PID.Compute(); // calculate PID outputs, output changes automatically
     motor2PID.Compute();
-// write new values to motor's
+    // write new values to motor's
     if (motor1SetSpeed ==0 ){
       motor1PID.SetOutputLimits(0,0);
     }
@@ -285,29 +289,32 @@
 
 
 bool kinematics(){
-    // calculate current x and Y
-    Xpos = (L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180);
-    Ypos = (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( (Xpos>Xmax && setXSpeed > 0 )|| \
-        (Xpos<Xmin && setXSpeed < 0 )|| \
-        (Ypos>Ymax && setYSpeed > 0 )|| \
-        (Ypos<Ymin && setYSpeed < 0 )   \
-    ){
-        motor1SetSpeed = 0;
-        motor2SetSpeed = 0;
-        return false;
-    }
-motor1SetSpeed = (180/PI)*((setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \
-    setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180)));
-motor2SetSpeed = (180/PI)*(-(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)));
+  // calculate current x and Y
+  Xpos = (L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180);
+  Ypos = (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( (Xpos>Xmax && setXSpeed > 0 )|| \
+      (Xpos<Xmin && setXSpeed < 0 )|| \
+      (Ypos>Ymax && setYSpeed > 0 )|| \
+      (Ypos<Ymin && setYSpeed < 0 )   \
+  ){
+      motor1SetSpeed = 0;
+      motor2SetSpeed = 0;
+      return false;
+  }
+  motor1SetSpeed = (180/PI)*((setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \
+      setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180)));
+  motor2SetSpeed = (180/PI)*(-(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)));
     return true;
+}
 
+void runPump(){
+  pumpSwitch.write(enablePump);
 }
\ No newline at end of file