Revision 94:28e274481b60, committed 2015-10-21

Comitter:

annesteenbeek

Date:

Wed Oct 21 12:03:43 2015 +0000

Parent:

93:a8898eb80edc

Parent:

92:12e2e57e900a

Child:

95:94f02d01ebdf

Commit message:

attempted merge

Changed in this revision

--- a/actuators.cpp	Wed Oct 21 13:16:24 2015 +0200
+++ b/actuators.cpp	Wed Oct 21 12:03:43 2015 +0000
@@ -8,7 +8,7 @@
 #include "buttons.h"
     // functions for controlling the motors
     bool motorsEnable = false;
-    bool safetyOn = false; // start with safety off for calibration
+    bool safetyOn = true; // start with safety off for calibration
 
 
     double encoder1Counts = 0;
@@ -177,11 +177,21 @@
 }
 
 void calibrateMotors(){
+<<<<<<< local
    safetyOn = false;
    redLed.write(0); greenLed.write(1); blueLed.write(1);
    while (calibrating1 || calibrating2){
        if (calibrating1){
+=======
+    safetyOn = false;
+    bool calibrating1 = true;
+    bool calibrating2 = true;
+    double motorCalSpeed = 10; // deg/sec
+    while (calibrating1 || calibrating2){
+        if (calibrating1){
+>>>>>>> other
             motor1SetSpeed = motorCalSpeed; // deg/sec 
+<<<<<<< local
             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
@@ -218,6 +228,27 @@
        }
 
    }
+=======
+            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);
+    }
+>>>>>>> other
     safetyOn = true; // turn safety on after callibration
 }
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/actuators.cpp.orig	Wed Oct 21 12:03:43 2015 +0000
@@ -0,0 +1,231 @@
+#include "actuators.h"
+#include "PID.h"
+#include "mbed.h"
+#include "config.h"
+#include "encoder.h"
+#include "HIDScope.h"
+#include "Servo.h"
+#include "buttons.h"
+    // functions for controlling the motors
+    bool motorsEnable = false;
+    bool safetyOn = false; // 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 motor1Pos = 0;
+    double motor2Pos = 0;
+
+    double motor1Speed = 0;
+    double motor2Speed = 0;
+
+    double motor1SetSpeed = 0;
+    double motor2SetSpeed = 0;
+
+    double servoPos = 0;
+
+    double motor1PWM = 0;
+    double motor2PWM = 0;
+
+    // Set PID values
+    double Kp1 = 1; 
+    double Ki1 = 0; 
+    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;
+
+    // for Calibration:
+    bool calibrating1 = true;
+    bool calibrating2 = false;
+    double motorCalSpeed = 10; // deg/sec
+    double returnSpeed = -10;
+    bool springHit = false;
+    float lastCall = 0;
+
+
+    // Create object instances
+    // Safety Pin
+    DigitalIn safetyIn(safetyPin);
+    
+    // Initialze motors
+    PwmOut motor1(motor1PWMPin);
+    PwmOut motor2(motor2PWMPin);
+
+    // Initialize encoders
+    Encoder encoder1(enc1A, enc1B);
+    Encoder encoder2(enc2A, enc2B);
+
+    // 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);
+
+    Servo servo(servoPin);
+    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);
+    
+    // start the timer
+    t.start();
+}
+
+
+void motorControl(){
+    // 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
+
+        encoder1Counts = encoder1.getPosition();
+        encoder2Counts = encoder2.getPosition();
+
+
+        motor1Pos = -((encoder1Counts/32)/131.25)*360;
+        motor2Pos = -((encoder2Counts/32)/131.25)*360;
+
+        // 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;
+        
+    // 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
+        writeMotors();
+    }else{
+        // write 0 to motors
+        motor1.write(0);
+        motor2.write(0);
+    }
+}
+
+void writeMotors(){
+    motor1PID.Compute(); // calculate PID outputs, output changes automatically
+    motor2PID.Compute();
+// write new values to motor's
+    if (motor1SetSpeed > 0 ){ // CCW rotation 
+        direction1 = false;
+        motor1PID.SetOutputLimits(0,1); // change pid output direction
+    }else{
+        direction1 = true; // CW rotation
+        motor1PID.SetOutputLimits(-1,0);
+    }
+    if (motor2SetSpeed > 0 ){ // CCW rotation 
+        direction2 = false;
+        motor2PID.SetOutputLimits(0,1);
+    }else{
+        direction2 = true; // CW rotation
+        motor2PID.SetOutputLimits(-1,0);
+    }
+    motor1Dir.write(direction1);
+    motor2Dir.write(direction2);
+
+    motor1.write(abs(motor1PWM));
+    motor2.write(abs(motor2PWM));
+}
+
+void servoControl(){
+    // use potMeter Value to set servo angle
+    if (motorsEnable){
+        servo.write(servoPos);
+    }
+    // (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.
+    
+}
+
+void calibrateMotors(){
+   safetyOn = false;
+   redLed.write(0); greenLed.write(1); blueLed.write(1);
+   while (calibrating1 || calibrating2){
+       if (calibrating1){
+            motor1SetSpeed = motorCalSpeed; // deg/sec 
+            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){
+           writeMotors();
+           lastCall = now;
+       }
+
+   }
+    safetyOn = true; // turn safety on after callibration
+}
+
+
+void safety(){
+    if (safetyOn){
+        if (safetyIn.read() != 1){
+            motorsEnable = false;
+        }
+    }
+}

--- a/config.h	Wed Oct 21 13:16:24 2015 +0200
+++ b/config.h	Wed Oct 21 12:03:43 2015 +0000
@@ -1,11 +1,17 @@
 // All the definitions and variables
+<<<<<<< local
 // #define TUNEPID  // set to switch between normal opperation or tuning PID
 // #define TUNEEMG // set hdiscope for EMG
 #define TUNEPWM // set hidscope for rest
+=======
+
+>>>>>>> other
 // Analog inputs for EMG
 #define EMG1in          A0
 #define EMG2in          A1
 
+#define startPin		PTC6
+
 // Analog inputs for potmeters
 #define pot1Pin         A2
 #define pot2Pin         A3
@@ -14,6 +20,10 @@
 
 #define button1Pin      D0
 #define button2Pin      D1
+<<<<<<< local
+=======
+#define button3Pin      PTC6
+>>>>>>> other
 
 // Servo
 #define servoPin        D9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/config.h.orig	Wed Oct 21 12:03:43 2015 +0000
@@ -0,0 +1,34 @@
+// All the definitions and variables
+// #define TUNEPID  // set to switch between normal opperation or tuning PID
+// #define TUNEEMG // set hdiscope for EMG
+#define TUNEPWM // set hidscope for rest
+// Analog inputs for EMG
+#define EMG1in          A0
+#define EMG2in          A1
+
+// Analog inputs for potmeters
+#define pot1Pin         A2
+#define pot2Pin         A3
+
+#define safetyPin		D2
+
+#define button1Pin      D0
+#define button2Pin      D1
+
+// Servo
+#define servoPin        D9
+
+// MOTORS
+#define motor2DirPin    D4
+#define motor2PWMPin    D5
+
+#define motor1DirPin    D7
+#define motor1PWMPin    D6
+
+#define enc1A           D11
+#define enc1B           D10
+#define enc2A           D13
+#define enc2B           D12
+
+// Motor control constants
+#define pwm_frequency 50000

--- a/debug.cpp	Wed Oct 21 13:16:24 2015 +0200
+++ b/debug.cpp	Wed Oct 21 12:03:43 2015 +0000
@@ -6,6 +6,9 @@
 #include "config.h"
 // all the debugging functions
 
+// #define TUNEPID  // set to switch between normal opperation or tuning PID
+// #define TUNEEMG // set hdiscope for EMG
+#define TUNEPWM // set hidscope for rest
 
 #ifdef TUNEPID
 HIDScope scope(5);

--- a/emg.cpp	Wed Oct 21 13:16:24 2015 +0200
+++ b/emg.cpp	Wed Oct 21 12:03:43 2015 +0000
@@ -14,13 +14,7 @@
 // Define program constants
 const int     on = 0;                   // On-constant for LEDs for program readability
 const int     off = 1;                  // Off-constant for LEDs for program readability
-const int     sample = 0;               // Constant for mode switching for program readability
-const int     normalize = 1;            // Constant for mode switching for program readability
-
 bool ledsEnable = false;
-//**********************************************************************************************
-bool mode               = normalize;         // Set program mode
-//**********************************************************************************************
 
 // Initialize sampling constants
 double emg_val1 = 0, emg_val2 = 0, emg_filt_val1 = 0, emg_filt_val2 = 0;
@@ -214,13 +208,14 @@
 }
 
 
-void readEMG(){
-    if(mode==normalize && normalizing_timer.read_ms() == 0) {   // Start normalizing timer
+void readEMG()
+{
+    if(emg_mode==normalize && normalizing_timer.read_ms() == 0) {   // Start normalizing timer
         normalizing_timer.reset();
         normalizing_timer.start();
     }
 
-    if(mode ==normalize) {
+    if(emg_mode ==normalize) {
         emg_val1 = emg1.read();               // Sample EMG value 1 from AnalogIn
         emg_val2 = emg2.read();               // Sample EMG value 2 from AnalogIn
 
@@ -239,7 +234,7 @@
 
         // First normalizing step: channel 1
         if (normalizing_timer.read_ms() <= normalize_time && channel == 1) {
-            if (ledsEnable){
+            if (ledsEnable) {
                 redLed.write(off);
                 greenLed.write(on);
             }
@@ -249,13 +244,13 @@
             // Second normalizing step: wait time, switch channel
         } else if (normalizing_timer.read_ms() > normalize_time && channel == 1) {
             channel = 2;
-            if (ledsEnable){
+            if (ledsEnable) {
                 greenLed.write(off);
                 redLed.write(on);
             }
             // Third normalizing step: channel 2
         } else if (normalizing_timer.read_ms() >= (normalize_time + normalize_wait) && normalizing_timer.read_ms() <= (2*normalize_time + normalize_wait) && channel == 2) {
-            if (ledsEnable){
+            if (ledsEnable) {
                 redLed.write(off);
                 greenLed.write(on);
             }
@@ -266,17 +261,17 @@
         } else if (normalizing_timer.read_ms() > (2*normalize_time + normalize_wait)) {
             normalizing_timer.stop();
             normalizing_timer.reset();
-            if (ledsEnable){
+            if (ledsEnable) {
                 greenLed.write(off);
                 redLed.write(off);
                 blueLed.write(on);
             }
-            mode = sample;
+            emg_mode = sample;
         }
     }
 
 
-    if(mode ==sample) {
+    if(emg_mode ==sample) {
         emg_val1 = emg1.read();                             // Sample EMG value 1 from AnalogIn
         emg_val2 = emg2.read();                             // Sample EMG value 2 from AnalogIn
 
@@ -303,7 +298,7 @@
             EMG_check(thr_pass1, thr_pass2, x_velocity, y_velocity, z_velocity, emg_filt_val1, emg_filt_val2, EMG_timer.read_ms());
 
         } else {
-            if (ledsEnable){
+            if (ledsEnable) {
                 greenLed.write(off);
                 redLed.write(on);
                 blueLed.write(off);
@@ -312,8 +307,5 @@
     }
 
     // Graphical output to HIDScope for debugging/ program check
-
-
-
 }
 

--- a/emg.h	Wed Oct 21 13:16:24 2015 +0200
+++ b/emg.h	Wed Oct 21 12:03:43 2015 +0000
@@ -10,6 +10,10 @@
 extern DigitalOut  greenLed;
 extern DigitalOut  blueLed;
 
+extern const int normalize;
+extern const int sample;
+extern bool emg_mode;
+
 extern const double sample_frequency; 
 
 extern const int     normalize_time;
@@ -44,5 +48,6 @@
 extern const double a12_gain, a12_a1, a12_a2, a12_b0, a12_b1, a12_b2; 
 extern const double a13_gain, a13_a1,  a13_a2, a13_b0, a13_b1, a13_b2;
 
+
 void readEMG();
 #endif

--- a/main.cpp	Wed Oct 21 13:16:24 2015 +0200
+++ b/main.cpp	Wed Oct 21 12:03:43 2015 +0000
@@ -1,10 +1,10 @@
 /*
-   ________                      ____        __          __ 
+   ________                      ____        __          __
   / ____/ /_  ___  __________   / __ \____  / /_  ____  / /_
  / /   / __ \/ _ \/ ___/ ___/  / /_/ / __ \/ __ \/ __ \/ __/
-/ /___/ / / /  __(__  |__  )  / _, _/ /_/ / /_/ / /_/ / /_  
-\____/_/ /_/\___/____/____/  /_/ |_|\____/_.___/\____/\__/  
-                                                            
+/ /___/ / / /  __(__  |__  )  / _, _/ /_/ / /_/ / /_/ / /_
+\____/_/ /_/\___/____/____/  /_/ |_|\____/_.___/\____/\__/
+
 */
 
 #include "mbed.h"
@@ -14,48 +14,85 @@
 #include "debug.h"
 #include "emg.h"
 
+const int     sample = 0;               // Constant for EMG mode switching for program readability
+const int     normalize = 1;            // Constant for EMG mode switching for program readability
+
+bool start=false;
+bool emg_mode = normalize;              // Set EMG mode
+
 Ticker switchesTick, debugTick, motorTick, EMGTick, safetyTick;
 volatile bool switches_go=false, debug_go=false, motor_go=false, emg_go=false, safety_go=false;
 
-void switches_activate(){switches_go=true;};
-void debug_activate(){debug_go=true;};
-void motor_activate(){motor_go=true;};
-void emg_activate(){emg_go=true;};
-void safety_activate(){safety_go=true;};
+void switches_activate()
+{
+    switches_go=true;
+};
+void debug_activate()
+{
+    debug_go=true;
+};
+void motor_activate()
+{
+    motor_go=true;
+};
+void emg_activate()
+{
+    emg_go=true;
+};
+void safety_activate()
+{
+    safety_go=true;
+};
 
-double motorCall = 0.01; // set motor frequency global so it can be used for speed.
-int main(){
-motorInit();    
-calibrateMotors(); // start calibration procedure
-
-switchesTick.attach(&switches_activate, 0.02f);
-debugTick.attach(&debug_activate, 0.03f);
-motorTick.attach(&motor_activate, motorCall);
-EMGTick.attach(&emg_activate, 0.005f);
-safetyTick.attach(&safety_activate, 0.001f);
-
+double motorCall = 0.005; // set motor frequency global so it can be used for speed.
+int main()
+{
+    DigitalIn startButton(startPin);
+    while(start == false) {
+        if(startButton.read()==0) {
+            start=true;
+        }
+    }
+    while (start == true) {
+        motorInit();
 
-    while (true) {
-        if(safety_go){
-            safety_go=false;
-            safety();
-        }
-        if(emg_go){
-            emg_go=false;
-            readEMG();
+        EMGTick.attach(&emg_activate, 0.005f);
+        switchesTick.attach(&switches_activate, 0.02f);
+        debugTick.attach(&debug_activate, 0.03f);
+        motorTick.attach(&motor_activate, motorCall);
+        safetyTick.attach(&safety_activate, 0.001f);
+
+        while (emg_mode == normalize) {
+            if(emg_go) {
+                emg_go=false;
+                readEMG();
+            }
         }
-        if(switches_go){
-            switches_go=false;
-            checkSwitches();
-        }
-        if(debug_go){
-            debug_go=false;
-            debugProcess();
-        }
-        if(motor_go){
-            motor_go=false;
-            motorControl();
-        // servoControl();
+
+        calibrateMotors();              // start calibration procedure
+
+        while (true) {
+            if(safety_go) {
+                safety_go=false;
+                safety();
+            }
+            if(emg_go) {
+                emg_go=false;
+                readEMG();
+            }
+            if(switches_go) {
+                switches_go=false;
+                checkSwitches();
+            }
+            if(debug_go) {
+                debug_go=false;
+                debugProcess();
+            }
+            if(motor_go) {
+                motor_go=false;
+                motorControl();
+                // servoControl();
+            }
         }
     }
 }
\ No newline at end of file