Diff: actuators.cpp

Revision:

98:25528494287d

Parent:

91:6bbbbc6643c8

Child:

99:7030e9790b1d

--- a/actuators.cpp	Thu Oct 22 07:16:59 2015 +0000
+++ b/actuators.cpp	Thu Oct 22 10:06:46 2015 +0200
@@ -4,7 +4,7 @@
 #include "config.h"
 #include "encoder.h"
 #include "HIDScope.h"
-#include "Servo.h"
+
     // functions for controlling the motors
     bool motorsEnable = false;
     bool safetyOn = false; // start with safety off for calibration
@@ -46,6 +46,14 @@
     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;
+
     // Create object instances
     // Safety Pin
     DigitalIn safetyIn(safetyPin);
@@ -54,6 +62,10 @@
     PwmOut motor1(motor1PWMPin);
     PwmOut motor2(motor2PWMPin);
 
+    // initialize Servo
+    PwmOut servo(servoPin);
+
+
     // Initialize encoders
     Encoder encoder1(enc1A, enc1B);
     Encoder encoder2(enc2A, enc2B);
@@ -66,7 +78,6 @@
     PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
     PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
 
-    Servo servo(servoPin);
     Timer t;  
     
 void motorInit(){
@@ -87,6 +98,10 @@
     // Turn PID on
     motor1PID.SetMode(AUTOMATIC);
     motor2PID.SetMode(AUTOMATIC);
+
+    // set servo period
+    servo.period(servoPeriod);               
+
     
     // start the timer
     t.start();
@@ -94,6 +109,11 @@
 
 
 void motorControl(){
+    // EMG signals to motor speeds
+    motor1SetSpeed = x_velocity;
+    motor2SetSpeed = y_velocity;
+    servoSpeed = z_velocity;
+
     // get encoder positions in degrees
         // 131.25:1 gear ratio
         // getPosition uses X2 configuration, so 32 counts per revolution
@@ -122,6 +142,7 @@
     if(motorsEnable){  // only run motors if switch is enabled
     // compute new PID parameters using setpoint angle speeds and encoder speed
         writeMotors();
+        servoControl();
     }else{
         // write 0 to motors
         motor1.write(0);
@@ -155,44 +176,62 @@
 }
 
 void servoControl(){
-    // use potMeter Value to set servo angle
-    if (motorsEnable){
-        servo.write(servoPos);
+    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;
+        }
     }
-    // (optionaly calculate xy position to keep balloon in position)
-        // calculate z position using angle
-        // calculate x y translation of endpoint
-        // find new x and y speed.
-    
+    servoPulsewidth = 0.0015 + (servo_pos/90)*0.001; 
+    servo.pulsewidth(servoPulsewidth);
 }
 
 void calibrateMotors(){
-//    safetyOn = false;
-//    bool calibrating1 = true;
-//    bool calibrating2 = true;
-//    double motorCalSpeed = 10; // deg/sec
-//    while (calibrating1 || calibrating2){
-//        if (calibrating1){
-//            motor1SetSpeed = motorCalSpeed; // deg/sec 
-//            if(safetyIn.read() !=1){ // check if arm reached safety position
-//                calibrating1 = false; // done 
-//                motor1SetSpeed = 0; // brake motor
-//                encoder1.setPosition(0); // set motor 1 cal angle
-//                // move away
-//            }
-//        }
-//        if (calibrating1){
-//            motor2SetSpeed = motorCalSpeed; // deg/sec 
-//            if(safetyIn.read() !=1){ // check if arm reached safety position
-//                calibrating2 = false; // done 
-//                motor2SetSpeed = 0; // brake motor
-//                encoder2.setPosition(0); // set motor 2 cal angle
-//                // move away
-//            }
-//        }
-//        writeMotors();
-//        wait(0.01f);
-//    }
+   safetyOn = false; // safety springs off
+   motorsEnable = true; // motors on
+   redLed.write(0); greenLed.write(1); blueLed.write(1);
+   while (calibrating1 || calibrating2){
+       if (calibrating1){
+            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
+               motor1SetSpeed = returnSpeed; // move away
+               springHit = true;
+           }else{ 
+               if(springHit){ // if hit and after is no longer touching spring
+                motor1SetSpeed = 0;
+                springHit = false;
+                calibrating1 = false;
+                calibrating2 = true; // start calibrating 2
+               }
+           }
+       }
+       if (calibrating2){
+            motor2SetSpeed = motorCalSpeed; 
+            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
+               motor2SetSpeed = returnSpeed; // move away
+               springHit = true;
+           }else{ 
+               if(springHit){ // if hit and after is no longer touching spring
+                motor2SetSpeed = 0;
+                springHit = false;
+                calibrating2 = false; // stop calibrating 2
+               }
+           }
+       }
+       now = t.read(); // call motor using timer instead of wait
+       if(now - lastCall > motorCall){
+           motorControl();
+           lastCall = now;
+       }
+
+   }
+    motorsEnable = false // turn motor's off again
     safetyOn = true; // turn safety on after callibration
 }