Diff: actuators.cpp

Revision:

106:1773bf7b95c5

Parent:

100:222c27f55b85

Child:

107:de47331612d9

--- a/actuators.cpp	Thu Oct 22 08:51:50 2015 +0000
+++ b/actuators.cpp	Mon Oct 26 12:46:29 2015 +0100
@@ -5,9 +5,13 @@
 #include "encoder.h"
 #include "HIDScope.h"
 
+    // Motor control constants
+    #define pwm_frequency 50000 // still High, could be lowered
+    #define PI 3.14159265
+
     // functions for controlling the motors
     bool motorsEnable = false;
-    bool safetyOn = true; // start with safety off for calibration
+    bool safetyOn = true; 
 
 
     double encoder1Counts = 0;
@@ -25,8 +29,6 @@
     double motor1SetSpeed = 0;
     double motor2SetSpeed = 0;
 
-    double servoPos = 0;
-
     double motor1PWM = 0;
     double motor2PWM = 0;
 
@@ -199,7 +201,7 @@
     servo.pulsewidth(servoPulsewidth);
 }
 
-void calibrateMotors(){
+bool calibrateMotors(){
    safetyOn = false; // safety springs off
    motorsEnable = true; // motors on
    redLed.write(0); greenLed.write(1); blueLed.write(1);
@@ -243,6 +245,7 @@
    }
     motorsEnable = false; // turn motor's off again
     safetyOn = true; // turn safety on after callibration
+    return true; // return true wehn finished
 }
 
 
@@ -253,3 +256,32 @@
         }
     }
 }
+
+
+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