Diff: actuators.cpp

Revision:

104:750d7e13137d

Parent:

103:4a37d19e8fcc

Child:

105:663b73bb2f81

--- a/actuators.cpp	Thu Oct 22 14:26:08 2015 +0000
+++ b/actuators.cpp	Fri Oct 23 12:17:29 2015 +0000
@@ -6,124 +6,126 @@
 #include "HIDScope.h"
 #include "buttons.h"
 
-    // functions for controlling the motors
-    bool motorsEnable = false;
-    bool safetyOn = true; // start with safety off for calibration
+// functions for controlling the motors
+bool motorsEnable = false;
+bool safetyOn = true; // start with safety off for calibration
 
 
-    double encoder1Counts = 0;
-    double encoder2Counts = 0;
-
-    bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
-    bool direction2 = false;
+double encoder1Counts = 0;
+double encoder2Counts = 0;
 
-    double motor1Pos = 0;
-    double motor2Pos = 0;
+bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
+bool direction2 = false;
 
-    double motor1Speed = 0;
-    double motor2Speed = 0;
+double motor1Pos = 0;
+double motor2Pos = 0;
 
-    double motor1SetSpeed = 0;
-    double motor2SetSpeed = 0;
+double motor1Speed = 0;
+double motor2Speed = 0;
 
-    double servoPos = 0;
-
-    double motor1PWM = 0;
-    double motor2PWM = 0;
+double motor1SetSpeed = 0;
+double motor2SetSpeed = 0;
 
-    // Set PID values
-    double Kp1 = 0.008; 
-    double Ki1 = 0.08; 
-    double Kd1 = 0;
+double servoPulsewidth;
+double servoPos = 0;
 
-    double Kp2 = 0.008; 
-    double Ki2 = 0.08; 
-    double Kd2 = 0;
+double motor1PWM = 0;
+double motor2PWM = 0;
 
-    double motor1PrevCounts = 0;
-    double motor2PrevCounts = 0;
-    double prevTime = 0;
-    double now = 0;
-    double timechange;
-    bool pidOut = 0;
+// Set PID values
+double Kp1 = 0.008;
+double Ki1 = 0.08;
+double Kd1 = 0;
+
+double Kp2 = 0.008;
+double Ki2 = 0.08;
+double Kd2 = 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 = 15;
-    double scaleXSpeed = 10;
-    double scaleYSpeed = 20;
-    double scaleZSpeed = 1;
-    
-    
-    
-    // Set calibration values
-    double motorCalSpeed = 10; // deg/sec
-    double returnSpeed = -10;
-    bool springHit = false;
-    float lastCall = 0;
-    bool calibrating1 = true;
-    bool calibrating2 = false;
+double motor1PrevCounts = 0;
+double motor2PrevCounts = 0;
+double prevTime = 0;
+double now = 0;
+double timechange;
+bool pidOut = 0;
 
-    // Create object instances
-    // Safety Pin
-    DigitalIn safetyIn(safetyPin);
-    
-    // Initialze motors
-    PwmOut motor1(motor1PWMPin);
-    PwmOut motor2(motor2PWMPin);
-
-    // initialize Servo
-    PwmOut servo(servoPin);
+// 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 * servoCall;     // Servo postion increment per cycle
+double servo_pos = 0;
+double servoSpeed = -1;
+double scaleXSpeed = 10;
+double scaleYSpeed = 20;
+double scaleZSpeed = 1;
 
 
-    // Initialize encoders
-    Encoder encoder1(enc1A, enc1B);
-    Encoder encoder2(enc2A, enc2B);
 
-    // Set direction pins     
-    DigitalOut motor1Dir(motor1DirPin);
-    DigitalOut motor2Dir(motor2DirPin);  
+// Set calibration values
+double motorCalSpeed = 10; // deg/sec
+double returnSpeed = -10;
+bool springHit = false;
+float lastCall = 0;
+bool calibrating1 = true;
+bool calibrating2 = false;
+
+// Create object instances
+// Safety Pin
+DigitalIn safetyIn(safetyPin);
+
+// Initialze motors
+PwmOut motor1(motor1PWMPin);
+PwmOut motor2(motor2PWMPin);
 
-    // create PID instances
-    PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
-    PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
+// initialize Servo
+PwmOut servo(servoPin);
+
+
+// Initialize encoders
+Encoder encoder1(enc1A, enc1B);
+Encoder encoder2(enc2A, enc2B);
 
-    Timer t;  
-    
-void motorInit(){
-    
+// Set direction pins
+DigitalOut motor1Dir(motor1DirPin);
+DigitalOut motor2Dir(motor2DirPin);
+
+// create PID instances
+PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
+PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
+
+Timer t;
+
+void motorInit()
+{
+
     motor1Dir.write(direction1);
     motor2Dir.write(direction2);
 
     // Set motor PWM period
     motor1.period(1/pwm_frequency);
     motor2.period(1/pwm_frequency);
-    
+
     motor1PID.SetSampleTime(motorCall);
     motor2PID.SetSampleTime(motorCall);
-    
+
     motor1PID.SetOutputLimits(0,1);
     motor2PID.SetOutputLimits(0,1);
-    
+
     // Turn PID on
     motor1PID.SetMode(AUTOMATIC);
     motor2PID.SetMode(AUTOMATIC);
 
     // set servo period
-    servo.period(servoPeriod);               
+    servo.period(servoPeriod);
 
-    
+
     // start the timer
     t.start();
 }
 
 
-void motorControl(){
+void motorControl()
+{
     // EMG signals to motor speeds
 //        motor1SetSpeed = x_velocity*scaleXSpeed;
 //        motor2SetSpeed = y_velocity*scaleYSpeed;
@@ -131,131 +133,146 @@
 
 
     // get encoder positions in degrees
-        // 131.25:1 gear ratio
-        // getPosition uses X2 configuration, so 32 counts per revolution
-        // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive
+    // 131.25:1 gear ratio
+    // getPosition uses X2 configuration, so 32 counts per revolution
+    // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive
 
-        encoder1Counts = encoder1.getPosition();
-        encoder2Counts = encoder2.getPosition();
+    encoder1Counts = encoder1.getPosition();
+    encoder2Counts = encoder2.getPosition();
 
 
-        motor1Pos = -((encoder1Counts/32)/131.25)*360;
-        motor2Pos = -((encoder2Counts/32)/131.25)*360;
+    motor1Pos = -((encoder1Counts/32)/131.25)*360;
+    motor2Pos = -((encoder2Counts/32)/131.25)*360;
 
-        // check if motor's are within rotational boundarys
+    // check if motor's are within rotational boundarys
     // get  encoder speeds in deg/sec
-        now = t.read(); 
-        timechange = (now - prevTime);
-        motor1Speed = -((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange;
-        motor2Speed = -((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange;
-        prevTime = now;
-        motor1PrevCounts = encoder1Counts;
-        motor2PrevCounts = encoder2Counts;
-        
+    now = t.read();
+    timechange = (now - prevTime);
+    motor1Speed = -((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange;
+    motor2Speed = -((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange;
+    prevTime = now;
+    motor1PrevCounts = encoder1Counts;
+    motor2PrevCounts = encoder2Counts;
+
     // calculate motor setpoint speed in deg/sec from setpoint x/y speed
 
-        
-    if(motorsEnable){  // only run motors if switch is enabled
-    // compute new PID parameters using setpoint angle speeds and encoder speed
+
+    if(motorsEnable) { // only run motors if switch is enabled
+        // compute new PID parameters using setpoint angle speeds and encoder speed
         writeMotors();
-        servoControl();
-    }else{
+
+    } else {
         // write 0 to motors
         motor1.write(0);
         motor2.write(0);
     }
 }
 
-void writeMotors(){
+void writeMotors()
+{
     motor1PID.Compute(); // calculate PID outputs, output changes automatically
     motor2PID.Compute();
 // write new values to motor's
-    if (motor1SetSpeed > 0 ){ // CCW rotation 
+    if (motor1SetSpeed > 0 ) { // CCW rotation
         direction1 = false;
         motor1PID.SetOutputLimits(0,1); // change pid output direction
-    }else{
+    } else {
         direction1 = true; // CW rotation
         motor1PID.SetOutputLimits(-1,0);
     }
-    if (motor2SetSpeed > 0 ){ // CCW rotation 
+    if (motor2SetSpeed > 0 ) { // CCW rotation
         direction2 = false;
         motor2PID.SetOutputLimits(0,1);
-    }else{
+    } else {
         direction2 = true; // CW rotation
         motor2PID.SetOutputLimits(-1,0);
     }
     motor1Dir.write(direction1);
     motor2Dir.write(direction2);
 
-    motor1.write(abs(motor1PWM));
-    motor2.write(abs(motor2PWM));
+   // motor1.write(abs(motor1PWM));
+ //   motor2.write(abs(motor2PWM));
 }
 
-void servoControl(){
+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) {
+    } 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);
+    servoPulsewidth = 0.0015 + (servo_pos/90)*0.001;
+    if(motorsEnable) {
+        servo.pulsewidth(servoPulsewidth);
+    } else if (!motorsEnable) {
+        servo.pulsewidth(0.0017);
+    }
 }
 
-void calibrateMotors(){
-   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;
-       }
+void calibrateMotors()
+{
+    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
 }
 
 
-void safety(){
-    if (safetyOn){
-        if (safetyIn.read() != 1){
+void safety()
+{
+    if (safetyOn) {
+        if (safetyIn.read() != 1) {
             motorsEnable = false;
+            redLed.write(!redLed.read());
         }
     }
 }