Diff: actuators.cpp

Revision:

105:663b73bb2f81

Parent:

104:750d7e13137d

--- a/actuators.cpp	Fri Oct 23 12:17:29 2015 +0000
+++ b/actuators.cpp	Mon Oct 26 11:25:25 2015 +0000
@@ -6,126 +6,124 @@
 #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;
+    double encoder1Counts = 0;
+    double encoder2Counts = 0;
+
+    bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
+    bool direction2 = false;
 
-bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
-bool direction2 = false;
+    double motor1Pos = 0;
+    double motor2Pos = 0;
 
-double motor1Pos = 0;
-double motor2Pos = 0;
+    double motor1Speed = 0;
+    double motor2Speed = 0;
 
-double motor1Speed = 0;
-double motor2Speed = 0;
+    double motor1SetSpeed = 0;
+    double motor2SetSpeed = 0;
 
-double motor1SetSpeed = 0;
-double motor2SetSpeed = 0;
+    double servoPos = 0;
+
+    double motor1PWM = 0;
+    double motor2PWM = 0;
 
-double servoPulsewidth;
-double servoPos = 0;
+    // Set PID values
+    double Kp1 = 0.008; 
+    double Ki1 = 0.08; 
+    double Kd1 = 0;
 
-double motor1PWM = 0;
-double motor2PWM = 0;
+    double Kp2 = 0.008; 
+    double Ki2 = 0.08; 
+    double Kd2 = 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;
+    double motor1PrevCounts = 0;
+    double motor2PrevCounts = 0;
+    double prevTime = 0;
+    double now = 0;
+    double timechange;
+    bool pidOut = 0;
 
-double motor1PrevCounts = 0;
-double motor2PrevCounts = 0;
-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 = 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;
 
-// 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;
+    // Create object instances
+    // Safety Pin
+    DigitalIn safetyIn(safetyPin);
+    
+    // Initialze motors
+    PwmOut motor1(motor1PWMPin);
+    PwmOut motor2(motor2PWMPin);
+
+    // initialize Servo
+    PwmOut servo(servoPin);
 
 
+    // Initialize encoders
+    Encoder encoder1(enc1A, enc1B);
+    Encoder encoder2(enc2A, enc2B);
 
-// 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);
+    // Set direction pins     
+    DigitalOut motor1Dir(motor1DirPin);
+    DigitalOut motor2Dir(motor2DirPin);  
 
-// initialize Servo
-PwmOut servo(servoPin);
-
-
-// Initialize encoders
-Encoder encoder1(enc1A, enc1B);
-Encoder encoder2(enc2A, enc2B);
+    // create PID instances
+    PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
+    PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
 
-// 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()
-{
-
+    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;
@@ -133,146 +131,131 @@
 
 
     // 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();
-
-    } else {
+        servoControl();
+    }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;
-    if(motorsEnable) {
-        servo.pulsewidth(servoPulsewidth);
-    } else if (!motorsEnable) {
-        servo.pulsewidth(0.0017);
-    }
+    servoPulsewidth = 0.0015 + (servo_pos/90)*0.001; 
+    servo.pulsewidth(servoPulsewidth);
 }
 
-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());
         }
     }
 }