Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of Inverse_kinematics by
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 }
Generated on Fri Jul 29 2022 12:03:03 by
