Revision 7:af586102d80c, committed 2018-11-07

Comitter:

CasperK

Date:

Wed Nov 07 13:51:55 2018 +0000

Parent:

6:344e075c1221

Commit message:

Final code

Changed in this revision

--- a/Constants.h	Fri Nov 02 09:16:57 2018 +0000
+++ b/Constants.h	Wed Nov 07 13:51:55 2018 +0000
@@ -7,8 +7,8 @@
 AnalogIn potmeter2(A4);
 DigitalIn button1(D2);
 DigitalIn button2(D3);
-DigitalOut directionpin1(D4);
-DigitalOut directionpin2(D7);
+DigitalOut directionpin2(D4);
+DigitalOut directionpin1(D7);
 QEI motor1(D13,D12,NC, 32);
 QEI motor2(D11,D10,NC, 32);
 
@@ -41,6 +41,7 @@
 
 Ticker sample_timer;
 Ticker MotorsTicker;
+Ticker stateticker;
 Timer timer;
 
 //for testing purposes
@@ -48,7 +49,7 @@
 DigitalOut ledgreen(LED_GREEN);
 DigitalOut ledblue(LED_BLUE);
 //MODSERIAL pc(USBTX, USBRX);
-HIDScope scope(6);
+//HIDScope scope(6);
 
 bool    emg0Bool    = 0;        // I don't know if these NEED to be global, but when I tried to put them in they wouldn't work...
 int     emg0Ignore  = 0;
@@ -73,4 +74,48 @@
 volatile float pwm_value1 = 0.0;
 volatile float pwm_value2 = 0.0;
 
+const float C1 = 3.0; //motor 1 gear ratio
+const float C2 = 0.013; //motor 2 gear ratio/radius of the circular gear in m
+const float length = 0.300; //length in m (placeholder)
+const float Timestep = 0.01;
+
+volatile bool x_direction = true;
+
+volatile float x_position = length;
+volatile float y_position = 0.0;
+volatile float old_x_position;
+volatile float old_y_position;
+volatile float x_correction;
+volatile float old_x_correction;
+volatile float old_motor1_angle;
+volatile float old_motor2_angle;
+volatile float motor1_angle = 0.0; //sawtooth gear motor
+volatile float motor2_angle = 0.0; //rotational gear motor
+volatile float direction;
+
+//values of PID controller
+const float Kp = 5;
+const float Ki = 0.02;
+const float Kd = 0.05;
+volatile float Output1 = 0 ;      //Starting value
+volatile float Output2 = 0 ;      //Starting value
+volatile float P1 = 0;           //encoder value
+volatile float P2 = 0;
+volatile float e1 = 0 ;          //Starting value 
+volatile float e2 = 0 ;          //Starting value
+volatile float e3 = 0;  
+volatile float f1 = 0 ;          //Starting value 
+volatile float f2 = 0 ;          //Starting value
+volatile float f3 = 0; 
+volatile float df3 = 0;
+volatile float if3 = 0;
+volatile float de3 = 0;
+volatile float ie3 = 0;
+
+volatile float Output_Last1;      // Remember previous position
+volatile float Output_Last2;      // Remember previous position
+volatile float Y1 = 0;               // Value that is outputted to motor control
+volatile float Y2 = 0;               // Value that is outputted to motor control
+volatile float P_Last = 0;       // Starting position
+
 #endif
\ No newline at end of file

--- a/main.cpp	Fri Nov 02 09:16:57 2018 +0000
+++ b/main.cpp	Wed Nov 07 13:51:55 2018 +0000
@@ -1,28 +1,166 @@
 #include "mbed.h"
 #include <stdio.h>
 #include "QEI.h"
-#include "HIDScope.h"
+//#include "HIDScope.h"
 //#include "MODSERIAL.h"
 #include "BiQuad.h"
 #include "math.h"
 #include "Constants.h"
+
 #define IGNORECOUNT 100
+#define PI 3.141592f //65358979323846  // pi 
+
+void yDirection() {
+    //direction of the motion
+    if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
+        direction = -1;
+    }
+    else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right
+        direction = 1;
+    }
+
+    if (emg0Bool || emg1Bool){  
+        //correction from motor 1 to keep x position the same
+        
+        
+        //calculating the motion
+        old_y_position = y_position;
+        y_position = old_y_position + (0.0003f * direction);
+        
+            
+        old_motor1_angle = motor1_angle;
+        motor1_angle = asin( y_position / length )* C1;     //saw tooth motor angle in rad
+        if (y_position > 0.29f){
+            y_position = 0.29f;
+        }
+        else if(y_position <0){
+            y_position = 0;
+        }
+        //correction on x- axis
+        old_x_correction = x_correction;
+        x_correction = old_x_correction - (length * cos(old_motor1_angle / C1)- length * cos(motor1_angle/ C1)); // old x + correction
+        if (x_position > 0.35f- x_correction ){
+            x_position = 0.35f- x_correction;
+        }
+        else if (x_position-x_correction < 0.0f){
+            x_position = 0.0f;
+        }
+        
+        motor2_angle = ( x_position + x_correction - (length * cos(motor1_angle /C1 ))) / C2;
+        //reset the boolean, for demo purposes
+        emg2Bool = false; 
+    }
+}
+
+void xDirection () {      
+    //if the button is pressed, reverse the y direction
+    if (!button2) {
+        x_direction = !x_direction;
+        wait(0.5f);
+        }     
+        
+    if (emg2Bool) { //if w is pressed, move up/down
+        //direction of the motion
+        if (x_direction) { 
+            direction = 1.0f;
+        }
+        else if (!x_direction) {
+            direction = -1.0f;
+        }
+        
+        //calculating the motion
+        old_x_position = x_position;
+        x_position = old_x_position + (0.0003f * direction);
+        if (x_position > 0.35f- x_correction ){
+            x_position = 0.35f- x_correction;
+        }
+        else if (x_position-x_correction < 0.0f){
+            x_position = 0.0f;
+        }
+
+        motor2_angle =( x_position + x_correction - (length * cos( motor1_angle / C1 ))) /C2 ; // sawtooth-gear motor angle in rad
+        
+        //reset the boolean, for demo purposes
+        emg2Bool = false; 
+    }
+}
+
+void PIDController1() {  
+    P1 = motor1.getPulses() / 8400.0f * 2.0f * PI;    //actual motor angle in rad  
+    e1 = e2;
+    e2 = e3;
+    e3 = motor1_angle - P1;
+    de3 = (e3-e2)/Timestep;
+    ie3 = ie3 + e3*Timestep;
+    Output1 = Kp * e3 + Ki * ie3 + Kd * de3;
+    
+    Y1 = 0.5f * Output1;
+    
+    if (Y1 >= 1){
+        Y1 = 1;
+    }
+    else if (Y1 <= -1){
+        Y1 = -1;
+    }       
+}
+
+void PIDController2() {   
+    P2 = motor2.getPulses() / 8400.0f * 2.0f*PI; // actual motor angle in rad
+    f2 = f3;
+    f3 = motor2_angle - P2;
+    df3 = (f3-f2)/Timestep;
+    if3 = if3 + f3*Timestep;
+    Output2 = Kp * f3 + Ki * if3 + Kd * df3;
+    Y2 = 0.5f * Output2;
+     
+    if (Y2 >= 1){
+        Y2 = 1;
+    }
+    else if (Y2 <= -1){
+        Y2 = -1;
+    }   
+}
+
+void ControlMotor1() {
+    if (Y1 > 0.0f) {
+        Y1 = 0.6f * Y1 + 0.4f;
+        directionpin1 = false;
+    } 
+    else if(Y1 < -0.0f){
+        Y1 = -0.4f + 0.6f * Y1;
+        directionpin1 = true;
+    }    
+
+    pwm_value1 = fabs(Y1);
+}
+
+void ControlMotor2() {
+    if (Y2 > 0.0f) {
+        Y2 = 0.6f * Y2 + 0.4f;
+        directionpin2 = true;
+    } 
+    else if(Y2 < -0.0f){
+        Y2 = -0.4f + 0.6f * Y2;
+        directionpin2 = false;
+    }
+
+    pwm_value2 = fabs(Y2);
+}
 
 /*
 * Calibration functions
 */
-
 void positionCalibration() {
     while(!button1) {
-        directionpin1 = true;
-        pwm_value1 = 0.7f;
+        directionpin2 = true;
+        pwmpin1 = 0.5f;
     }
-    pwm_value1 = 0.0f;
     while(!button2) {
-        directionpin2 = true;
-        pwm_value2 = 0.6f;
+        directionpin1 = true;
+        pwmpin2 = 0.4f;
     }
-    pwm_value2 = 0.0f;
+    pwmpin1 = 0.0f;
+    pwmpin2 = 0.0f;
     
     if(!next_switch) {
         CurrentState = Movement;
@@ -56,7 +194,7 @@
 }  
 
 void CalibrateEMG1(){          // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
-    ledgreen = !ledgreen;
+    ledred = !ledred;
     int C = 500;           // half a second at 1000Hz
     double A0=0, A1=0, A2=0, A3=0, A4=0;
     double emg1FiAbs;
@@ -76,11 +214,11 @@
         wait(0.001f);
     }
     Calibration1 = (A0+A1+A2+A3+A4)/5*0.4;  // average of the 5 highest values x 0,4 to create the threshold
-    ledgreen = !ledgreen;
+    ledred = !ledred;
 }  
 
 void CalibrateEMG2(){          // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
-    ledblue = !ledblue;
+    ledred = !ledred;
     int C = 500;           // half a second at 1000Hz
     double A0=0, A1=0, A2=0, A3=0, A4=0;
     double emg2FiAbs;
@@ -100,7 +238,7 @@
         wait(0.001f);
     }
     Calibration2 = (A0+A1+A2+A3+A4)/5*0.4;  // average of the 5 highest values x 0,4 to create the threshold
-    ledblue = !ledblue;
+    ledred = !ledred;
 }    
 
 /*
@@ -207,7 +345,12 @@
 * function that runs the Kinematics and PID controller
 */
 void movement() {
-    
+    xDirection(); //call the function to move in the y direction
+    yDirection(); //call the function to move in the x direction
+    PIDController1();
+    PIDController2();
+    ControlMotor1();
+    ControlMotor2();
 }
 
 void move_motors() {
@@ -227,6 +370,7 @@
     break;
     
     case PositionCalibration:
+    ledred = true;
     ledgreen = true;
     ledblue = false; //turn on the blue light
     positionCalibration();
@@ -242,20 +386,22 @@
 }
 
 void KillInterrupt() {
-    //turn off the motors
-    pwm_value1 = 0;
-    pwm_value2 = 0;
-    
     //detach all the tickers
     MotorsTicker.detach();
     sample_timer.detach();
+    stateticker.detach();
+    
+    //turn off the motors
+    pwm_value1 = 0.0f;
+    pwm_value2 = 0.0f;
+    pwmpin1 = 0.0f;
+    pwmpin2 = 0.0f;
     
     //burn the red light
     ledred = false;
     ledgreen = true;
     ledblue = true;
     
-    wait(2.0f); //give the person time to release the button
     bool b = true;
     
     //have the program get stuck in a while loop 
@@ -268,7 +414,9 @@
             b = false;
         }
     }
-    StateFunction(); //get back to the state function
+    wait(1.5f);
+    MotorsTicker.attach(&move_motors, Timestep);
+    stateticker.attach(StateFunction, Timestep);
 }
 
 int main()
@@ -283,11 +431,11 @@
     ledblue = true;
         
     kill_switch.fall(KillInterrupt); //attach the kill switch to the KillInterrupt function
-    MotorsTicker.attach(&move_motors, 0.02f); //ticker at 50Hz
+    MotorsTicker.attach(&move_motors, Timestep); //ticker at 50Hz
+    stateticker.attach(StateFunction, Timestep);
 
     CurrentState = EmgCalibration; //start at the calibration state
     
-    while (true) {
-        StateFunction(); //keep running the state machine       
+    while (true) {      
     }
 }
\ No newline at end of file