Diff: actuators.cpp

Revision:

107:de47331612d9

Parent:

106:1773bf7b95c5

Child:

109:026abd708dce

--- a/actuators.cpp	Mon Oct 26 12:46:29 2015 +0100
+++ b/actuators.cpp	Mon Oct 26 13:04:22 2015 +0100
@@ -4,6 +4,8 @@
 #include "config.h"
 #include "encoder.h"
 #include "HIDScope.h"
+#include "buttons.h"
+#include "Servo.h"
 
     // Motor control constants
     #define pwm_frequency 50000 // still High, could be lowered
@@ -46,16 +48,9 @@
     double prevTime = 0;
     double now = 0;
     double timechange;
-    bool pidOut = 0;
 
     // Set servo values
-    const double servoPeriod = 0.020;
-    const double servo_range = 20;  // Servo range (-range/ range) [deg]
-    const double servo_vel = 15;    // Servo velocity [deg/s]
-    const double servo_inc = servo_vel * motorCall;     // Servo postion increment per cycle
-    double servo_pos = 0;
-    double servoPulsewidth = 0.0015;
-    double servoSpeed = 0;
+    const int servoStartPos = 1300; // Servo ranges from 600ms(-90) to 2000ms(90), 1300 is 0 deg
     
     // Set calibration values
     double motorCalSpeed = 10; // deg/sec
@@ -74,7 +69,7 @@
     PwmOut motor2(motor2PWMPin);
 
     // initialize Servo
-    PwmOut servo(servoPin);
+    Servo servo(servoPin);
 
 
     // Initialize encoders
@@ -111,21 +106,21 @@
     motor2PID.SetMode(AUTOMATIC);
 
     // set servo period
-    servo.period(servoPeriod);               
+    servo.Enable(servoStartPos, 20000);               
 
-    
     // start the timer
     t.start();
 }
 
 
 void motorControl(){
-    // EMG signals to motor speeds
-    const double scaleVel = 20;
-    motor1SetSpeed = x_velocity*scaleVel;
-    motor2SetSpeed = y_velocity*scaleVel;
-    servoSpeed = z_velocity*scaleVel;
-
+    #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
@@ -188,17 +183,7 @@
 }
 
 void servoControl(){
-    if (servoSpeed > 0) {
-        if((servo_pos + servo_inc) <= servo_range) {       // If increment step does not exceed maximum range
-            servo_pos += servo_inc;
-        }
-    }else if (servoSpeed < 0) {
-        if((servo_pos - servo_inc) >= -servo_range) {       // If increment step does not exceed maximum range
-            servo_pos -= servo_inc;
-        }
-    }
-    servoPulsewidth = 0.0015 + (servo_pos/90)*0.001; 
-    servo.pulsewidth(servoPulsewidth);
+    servo.SetPosition(1300 + 700*servoSpeed);
 }
 
 bool calibrateMotors(){
@@ -259,29 +244,29 @@
 
 
 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));
+//     // 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