Wessel Klumperman / Mbed 2 deprecated Inverse_kinematics

Dependencies:   Encoder HIDScope MODSERIAL mbed

Fork of Inverse_kinematics by Casper Kroon

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "MODSERIAL.h"
00004 #include "HIDScope.h"
00005 #include "encoder.h"
00006 
00007 DigitalIn button(SW2);
00008 DigitalOut directionpin1(D7);
00009 DigitalOut directionpin2(D8);
00010 MODSERIAL pc(USBTX, USBRX);
00011 HIDScope scope(2);
00012 Encoder motor1(D13,D12);
00013 Encoder motor2(D13,D12); //has to be changed!!!
00014 
00015 //values of inverse kinematics
00016 volatile bool emg0Bool = false;
00017 volatile bool emg1Bool = false;
00018 volatile bool emg2Bool = false;
00019 volatile bool y_direction = true;
00020 volatile bool a;
00021 
00022 volatile float x_position = 0.0;
00023 volatile float y_position = 0.0;
00024 volatile float y_position2 = 0.0;
00025 volatile float old_y;
00026 volatile float old_y2;
00027 volatile float old_x;
00028 volatile float motor1_angle = 0.0; //circular gear motor
00029 volatile float motor2_angle = 0.0; //sawtooth gear motor
00030 volatile float direction;
00031 volatile char c;
00032 
00033 const float length = 0.300; //length in m (placeholder)
00034 const float C1 = 3; //motor 1 gear ratio
00035 const float C2 = 3; //motor 2 gear ratio/radius of the circular gear (placeholder)
00036 
00037 //values of PID controller
00038 const float Kp = 2;
00039 const float Ki = 0.2;
00040 const float Kd = 0;
00041 const float Timestep = 0.001;
00042 float G ;               //desired motor angle
00043 float Output1 = 0 ;      //Starting value
00044 float Output2 = 0 ;      //Starting value
00045 float P = 0;           //encoder value
00046 float e1 = 0 ;          //Starting value 
00047 float e2 = 0 ;          //Starting value
00048 float e3;  
00049 float f1 = 0 ;          //Starting value 
00050 float f2 = 0 ;          //Starting value
00051 float f3;     
00052 float Output_Last1;      // Remember previous position
00053 float Output_Last2;      // Remember previous position
00054 float Y1;                // Value that is outputted to motor control
00055 float Y2;
00056 float P_Last = 0;       // Starting position
00057 const float Max_Speed = 400;      //Max speed of the motor
00058       
00059 void xDirection() {
00060     //direction of the motion
00061     if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
00062         directionpin1 = true;
00063         directionpin2 = true;
00064         direction = -1;
00065     }
00066     else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right
00067         directionpin1 = false;
00068         directionpin2 = false;
00069         direction = 1;
00070     }
00071 
00072     if (emg0Bool || emg1Bool){
00073         //calculating the motion
00074         old_x = x_position;
00075         x_position = old_x + (0.1f * direction);
00076         motor1_angle = asin( x_position / (length * C1 ));     //rotational motor angle in rad
00077         
00078         old_y2 = y_position2;
00079         y_position2 = old_y2 + (0.1f * direction);
00080         motor2_angle = ( y_position / C2) + acos( x_position / length );     //sawtooth-gear motor angle in rad
00081     }
00082     
00083     //reset the booleans
00084     emg0Bool = false;
00085     emg1Bool = false;
00086 }
00087 
00088 void yDirection () {      
00089     if (emg2Bool) { //if w is pressed, move up/down
00090         //direction of the motion
00091         if (y_direction) { 
00092             directionpin2 = true;
00093             direction = 1;
00094         }
00095         else if (!y_direction) {
00096             directionpin2 = false;
00097             direction = -1;
00098         }
00099         
00100         //calculating the motion
00101         old_y = y_position;
00102         y_position = old_y + (0.1f * direction);
00103         motor2_angle = y_position / C2; // sawtooth-gear motor angle in rad
00104         
00105         //reset the boolean
00106         emg2Bool = false; 
00107     }
00108 }
00109 
00110 void PIDController1() {   
00111     P = motor1.getPosition() / 8400 * 360;        
00112     e1 = e2;
00113     e2 = e3;
00114     e3 = G - P;
00115     Output_Last1 = Output1;
00116     Output1 = Kp * (e3 - e2) + Output_Last1 +Ki * e3 + Kd * (e3 - 2*e2 + e1);
00117     Y1 = Output1;
00118      
00119     if (Output1 >= 1){
00120         Y1 = 1;
00121     }
00122     else if (Output1 <= -1){
00123         Y1 = -1;
00124     }   
00125     if (Y1 >= 0) {
00126         Y1 = 0.5f * Y1 + 0.5f;
00127     } 
00128     else {
00129     Y1 = 0.5f + 0.5f * Y1;
00130     }
00131     scope.set(0,Output1);
00132     scope.set(1,P);
00133     scope.send();
00134 }
00135 
00136 void PIDController2() {   
00137     P = motor2.getPosition() / 8400 * 360;        
00138     f1 = f2;
00139     f2 = f3;
00140     f3 = G - P;
00141     Output_Last2 = Output2;
00142     Output2 = Kp * (f3 - f2) + Output_Last2 +Ki * f3 + Kd * (f3 - 2*f2 + f1);
00143     Y2 = Output2;
00144      
00145     if (Output2 >= 1){
00146         Y2 = 1;
00147     }
00148     else if (Output2 <= -1){
00149         Y2 = -1;
00150     }    
00151     if (Y2 >= 0) {
00152         Y2 = 0.5f * Y2 + 0.5f;
00153     } 
00154     else {
00155     Y2 = 0.5f + 0.5f * Y2;
00156     }
00157 
00158     scope.set(0,Output2);
00159     scope.set(1,P);
00160     scope.send();
00161 }
00162 
00163 int main() {
00164     pc.baud(115200);
00165     pc.printf(" ** program reset **\n\r");
00166     
00167     while (true) {
00168         //if the button is pressed, reverse the y direction
00169         if (!button) {
00170             y_direction = !y_direction;
00171             wait(0.5);
00172         }
00173         
00174         //testing the code from keyboard imputs: a-d: left to right, w: forward/backwards
00175         a = pc.readable();
00176         if (a) {
00177             c = pc.getc();
00178             switch (c){
00179                 case 'a': //move to the left
00180                     emg0Bool = true; 
00181                     break;           
00182                 case 'd': //move to the right
00183                     emg1Bool = true;
00184                     break;            
00185                 case 'w': //move up/down
00186                     emg2Bool = true;
00187                     break;
00188             }
00189         }
00190         yDirection(); //call the function to move in the y direction
00191         xDirection(); //call the function to move in the x direction
00192         PIDController1();
00193         PIDController2();
00194         
00195         // print the motor angles and coordinates
00196         pc.printf("position: (%f, %f)\n\r", x_position, y_position);
00197         pc.printf("motor1 angle: %f\n\r", motor1_angle);
00198         pc.printf("motor2 angle: %f\n\r\n", motor2_angle);
00199 
00200         wait(0.5f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
00201     }
00202 }