Casper Berkhout / Mbed 2 deprecated BioRobotics_Main

Main control script for biorobotics project

Dependencies:   MODSERIAL QEI biquadFilter mbed

main.cpp@1:d7299175a12e, 2017-11-02 (annotated)

Committer:
CasperBerkhout
Date:
Thu Nov 02 08:57:41 2017 +0000
Revision:
1:d7299175a12e
Parent:
0:4141aef83f4b
Child:
2:c7369b41f7ae
Homing and PI control working. Kinematics not active

Who changed what in which revision?

UserRevisionLine numberNew contents of line
CasperBerkhout 0:4141aef83f4b 1 #include "QEI.h"
CasperBerkhout 0:4141aef83f4b 2 #include "math.h"
CasperBerkhout 0:4141aef83f4b 3 #include "mbed.h"
CasperBerkhout 0:4141aef83f4b 4 //#include "HIDScope.h" //set mbed library version to 119 for HIDScope to work
CasperBerkhout 0:4141aef83f4b 5 #include "MODSERIAL.h"
CasperBerkhout 0:4141aef83f4b 6 #include "BiQuad.h"
CasperBerkhout 0:4141aef83f4b 7
CasperBerkhout 0:4141aef83f4b 8 //left pot to set reference position.
CasperBerkhout 0:4141aef83f4b 9 //right pot to set Kp, right pot sets Ki when (right)button is pressed down
CasperBerkhout 0:4141aef83f4b 10
CasperBerkhout 0:4141aef83f4b 11
CasperBerkhout 0:4141aef83f4b 12 //--------------object creation------------------
CasperBerkhout 0:4141aef83f4b 13 Serial pc(USBTX, USBRX);
CasperBerkhout 0:4141aef83f4b 14 //Use X4 encoding.
CasperBerkhout 0:4141aef83f4b 15 //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
CasperBerkhout 0:4141aef83f4b 16 //Use X2 encoding by default.
CasperBerkhout 0:4141aef83f4b 17 QEI enc1(D13, D12, NC, 32); //enable the encoder
CasperBerkhout 0:4141aef83f4b 18 QEI enc2(D15, D14, NC, 32); //enable the encoder
CasperBerkhout 0:4141aef83f4b 19 PwmOut M1_speed(D6);
CasperBerkhout 0:4141aef83f4b 20 PwmOut M2_speed(D5);
CasperBerkhout 0:4141aef83f4b 21 DigitalOut M1_direction(D7);
CasperBerkhout 0:4141aef83f4b 22 DigitalOut M2_direction(D4);
CasperBerkhout 0:4141aef83f4b 23 AnalogIn potmeter(A0); //left pot
CasperBerkhout 0:4141aef83f4b 24 AnalogIn potmeter2(A1); //right pot
CasperBerkhout 1:d7299175a12e 25 InterruptIn button1(D2); //hardware interrupt for stopping motors - right button
CasperBerkhout 1:d7299175a12e 26 DigitalIn LimSW1(D9);
CasperBerkhout 1:d7299175a12e 27 DigitalIn LimSW2(D8);
CasperBerkhout 1:d7299175a12e 28 DigitalIn HomStart(D3); // - left button
CasperBerkhout 1:d7299175a12e 29
CasperBerkhout 0:4141aef83f4b 30 BiQuad bqlowpass(0, 0.611, 0.611, 1, 0.222);
CasperBerkhout 0:4141aef83f4b 31 //create scope objects - note: script won't run when HID usb port is not connected
CasperBerkhout 0:4141aef83f4b 32 //HIDScope scope(5); //set # of channels
CasperBerkhout 1:d7299175a12e 33
CasperBerkhout 1:d7299175a12e 34 Ticker motor_update1;
CasperBerkhout 1:d7299175a12e 35 Ticker motor_update2;
CasperBerkhout 1:d7299175a12e 36 Ticker error_update;
CasperBerkhout 0:4141aef83f4b 37
CasperBerkhout 0:4141aef83f4b 38
CasperBerkhout 0:4141aef83f4b 39 //-----------------variable decleration----------------
CasperBerkhout 0:4141aef83f4b 40 int pwm_freq = 500;
CasperBerkhout 0:4141aef83f4b 41 float set_speed;
CasperBerkhout 0:4141aef83f4b 42 float reference_pos;
CasperBerkhout 0:4141aef83f4b 43
CasperBerkhout 0:4141aef83f4b 44
CasperBerkhout 0:4141aef83f4b 45 float Arm1_home = 112.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
CasperBerkhout 0:4141aef83f4b 46 float Arm2_home = 19.0/180.0*3.1416;//home position of small link attached to base
CasperBerkhout 0:4141aef83f4b 47
CasperBerkhout 0:4141aef83f4b 48 double M1_home;
CasperBerkhout 0:4141aef83f4b 49 double M1_error_pos = 0;
CasperBerkhout 1:d7299175a12e 50 float M1_Kp = 2;
CasperBerkhout 1:d7299175a12e 51 float M1_Ki = 4;
CasperBerkhout 1:d7299175a12e 52 float M1_Kd = 0;
CasperBerkhout 0:4141aef83f4b 53 double M1_e_int=0;
CasperBerkhout 0:4141aef83f4b 54 double M1_e_prior=0;
CasperBerkhout 0:4141aef83f4b 55
CasperBerkhout 0:4141aef83f4b 56 double M2_home;
CasperBerkhout 0:4141aef83f4b 57 double M2_error_pos = 0;
CasperBerkhout 1:d7299175a12e 58 float M2_Kp = 0.3;
CasperBerkhout 0:4141aef83f4b 59 float M2_Ki = 0.1;
CasperBerkhout 1:d7299175a12e 60 float M2_Kd = 0;
CasperBerkhout 0:4141aef83f4b 61 double M2_e_int=0;
CasperBerkhout 0:4141aef83f4b 62 double M2_e_prior=0;
CasperBerkhout 0:4141aef83f4b 63
CasperBerkhout 0:4141aef83f4b 64 float Ts = 0.002; //500hz sample freq
CasperBerkhout 0:4141aef83f4b 65
CasperBerkhout 1:d7299175a12e 66 bool M1homflag;
CasperBerkhout 1:d7299175a12e 67 bool M2homflag;
CasperBerkhout 1:d7299175a12e 68 bool Homstartflag;
CasperBerkhout 1:d7299175a12e 69
CasperBerkhout 0:4141aef83f4b 70 void homing_system () {
CasperBerkhout 0:4141aef83f4b 71 LimSW1.mode(PullDown);
CasperBerkhout 0:4141aef83f4b 72 LimSW2.mode(PullDown);
CasperBerkhout 0:4141aef83f4b 73 M1_speed.write(0);
CasperBerkhout 0:4141aef83f4b 74 M2_speed.write(0);
CasperBerkhout 0:4141aef83f4b 75 M1_direction.write(0);
CasperBerkhout 1:d7299175a12e 76 M2_direction.write(1);
CasperBerkhout 0:4141aef83f4b 77
CasperBerkhout 1:d7299175a12e 78 while(1){
CasperBerkhout 1:d7299175a12e 79 if (HomStart.read() == 0){
CasperBerkhout 1:d7299175a12e 80 M1_speed.write(0.4);
CasperBerkhout 1:d7299175a12e 81 wait(0.5);
CasperBerkhout 1:d7299175a12e 82 M2_speed.write(0.3);
CasperBerkhout 1:d7299175a12e 83 pc.printf("starting M1 \n\r");
CasperBerkhout 1:d7299175a12e 84 }
CasperBerkhout 1:d7299175a12e 85 if(LimSW1.read() == 1){
CasperBerkhout 1:d7299175a12e 86
CasperBerkhout 1:d7299175a12e 87 M1_speed.write(0);
CasperBerkhout 1:d7299175a12e 88 M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 1:d7299175a12e 89 }
CasperBerkhout 1:d7299175a12e 90 if (LimSW2.read() == 1) {
CasperBerkhout 1:d7299175a12e 91 M2_speed.write(0);
CasperBerkhout 1:d7299175a12e 92
CasperBerkhout 1:d7299175a12e 93 M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 1:d7299175a12e 94 }
CasperBerkhout 0:4141aef83f4b 95
CasperBerkhout 1:d7299175a12e 96 if (LimSW1.read() == 1 && LimSW2.read() ==1) {
CasperBerkhout 1:d7299175a12e 97 pc.printf("Homing finished \n\r");
CasperBerkhout 1:d7299175a12e 98 M1_speed.write(0);
CasperBerkhout 1:d7299175a12e 99 M2_speed.write(0);
CasperBerkhout 1:d7299175a12e 100 wait(0.5);
CasperBerkhout 1:d7299175a12e 101 M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 1:d7299175a12e 102 M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 0:4141aef83f4b 103 break;
CasperBerkhout 1:d7299175a12e 104 }
CasperBerkhout 1:d7299175a12e 105 }
CasperBerkhout 1:d7299175a12e 106
CasperBerkhout 0:4141aef83f4b 107 }
CasperBerkhout 0:4141aef83f4b 108
CasperBerkhout 0:4141aef83f4b 109
CasperBerkhout 0:4141aef83f4b 110 void kinemtaica(double q1, double q2, double vdx, double vdy, double &q1_new, double &q2_new){
CasperBerkhout 0:4141aef83f4b 111 double q1_dot = (2.5*cos(q1+q2))/(sin(q2)) *vdx + (2.5*sin(q1+q2))/(sin(q2))*vdy;
CasperBerkhout 0:4141aef83f4b 112 double q2_dot = -(0.3*cos(q1+q2)+0.4*cos(q1))/(0.12*sin(q2))*vdx -(0.3*sin(q1+q2)+0.4*sin(q2))/(0.12*sin(q2))*vdy;
CasperBerkhout 0:4141aef83f4b 113 q1_new = q1 +q1_dot*Ts;
CasperBerkhout 0:4141aef83f4b 114 q2_new = q2 +q2_dot*Ts;
CasperBerkhout 0:4141aef83f4b 115 return;
CasperBerkhout 0:4141aef83f4b 116 }
CasperBerkhout 0:4141aef83f4b 117
CasperBerkhout 0:4141aef83f4b 118 float PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prior){ //PID calculator
CasperBerkhout 0:4141aef83f4b 119 e_int += Ts*e;
CasperBerkhout 0:4141aef83f4b 120 double e_diff = (e-e_prior)/Ts;
CasperBerkhout 0:4141aef83f4b 121 e_prior = e;
CasperBerkhout 0:4141aef83f4b 122 double e_diff_filter = bqlowpass.step(e_diff);
CasperBerkhout 0:4141aef83f4b 123 return(Kp*e+Ki*e_int+Kd*e_diff_filter);
CasperBerkhout 0:4141aef83f4b 124 }
CasperBerkhout 0:4141aef83f4b 125
CasperBerkhout 0:4141aef83f4b 126 void M1_control(){
CasperBerkhout 0:4141aef83f4b 127
CasperBerkhout 0:4141aef83f4b 128 //call PID func and set new motor speed
CasperBerkhout 0:4141aef83f4b 129 set_speed = PID(M1_error_pos,M1_Kp,M1_Ki,M1_Kd,Ts,M1_e_int,M1_e_prior);
CasperBerkhout 0:4141aef83f4b 130 if(set_speed > 0){
CasperBerkhout 0:4141aef83f4b 131 M1_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 132 M1_direction.write(0);
CasperBerkhout 0:4141aef83f4b 133 }
CasperBerkhout 0:4141aef83f4b 134 else if (set_speed < 0){
CasperBerkhout 0:4141aef83f4b 135 M1_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 136 M1_direction.write(1);
CasperBerkhout 0:4141aef83f4b 137 }
CasperBerkhout 0:4141aef83f4b 138 else{M1_speed.write(0);}
CasperBerkhout 1:d7299175a12e 139 pc.printf("Motor1 set speed = %f \n\r",set_speed);
CasperBerkhout 0:4141aef83f4b 140 }
CasperBerkhout 0:4141aef83f4b 141
CasperBerkhout 0:4141aef83f4b 142 void M2_control(){
CasperBerkhout 0:4141aef83f4b 143 set_speed = PID(M2_error_pos,M2_Kp,M2_Ki,M2_Kd,Ts,M2_e_int,M2_e_prior);
CasperBerkhout 0:4141aef83f4b 144 if(set_speed > 0){
CasperBerkhout 0:4141aef83f4b 145 M2_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 146 M2_direction.write(0);
CasperBerkhout 0:4141aef83f4b 147 }
CasperBerkhout 0:4141aef83f4b 148 else if (set_speed < 0){
CasperBerkhout 0:4141aef83f4b 149 M2_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 150 M2_direction.write(1);
CasperBerkhout 0:4141aef83f4b 151 }
CasperBerkhout 0:4141aef83f4b 152 else{M2_speed.write(0);}
CasperBerkhout 0:4141aef83f4b 153 }
CasperBerkhout 0:4141aef83f4b 154
CasperBerkhout 0:4141aef83f4b 155
CasperBerkhout 0:4141aef83f4b 156
CasperBerkhout 0:4141aef83f4b 157 void scopesend(){
CasperBerkhout 0:4141aef83f4b 158
CasperBerkhout 0:4141aef83f4b 159
CasperBerkhout 0:4141aef83f4b 160
CasperBerkhout 0:4141aef83f4b 161 }
CasperBerkhout 0:4141aef83f4b 162 void StopMotors(){
CasperBerkhout 0:4141aef83f4b 163 while(1){
CasperBerkhout 0:4141aef83f4b 164 M1_speed.write(0);
CasperBerkhout 0:4141aef83f4b 165 M2_speed.write(0);
CasperBerkhout 0:4141aef83f4b 166 }
CasperBerkhout 0:4141aef83f4b 167 }
CasperBerkhout 0:4141aef83f4b 168
CasperBerkhout 0:4141aef83f4b 169 void geterror(){
CasperBerkhout 0:4141aef83f4b 170 double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
CasperBerkhout 0:4141aef83f4b 171 double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
CasperBerkhout 0:4141aef83f4b 172
CasperBerkhout 0:4141aef83f4b 173 double M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!!
CasperBerkhout 0:4141aef83f4b 174 double M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!!
CasperBerkhout 0:4141aef83f4b 175
CasperBerkhout 0:4141aef83f4b 176 double q1 = M1_actual_pos;
CasperBerkhout 1:d7299175a12e 177 double q2 = 3.1416-M1_actual_pos-M2_actual_pos; //see drawing
CasperBerkhout 0:4141aef83f4b 178
CasperBerkhout 1:d7299175a12e 179 //double M1_reference_pos = 1+potmeter.read()*0.5*3.1416; //should cover the right range - radians
CasperBerkhout 1:d7299175a12e 180 double M1_reference_pos = 0.5*3.1416; //should cover the right range - radians
CasperBerkhout 1:d7299175a12e 181 double M2_reference_pos = 0.5+potmeter2.read()*0.25*3.1416;
CasperBerkhout 0:4141aef83f4b 182
CasperBerkhout 0:4141aef83f4b 183 M1_error_pos = M1_reference_pos - M1_actual_pos;
CasperBerkhout 1:d7299175a12e 184 //M2_error_pos = M2_reference_pos - M2_actual_pos;
CasperBerkhout 1:d7299175a12e 185 M2_error_pos = 0;
CasperBerkhout 0:4141aef83f4b 186 }
CasperBerkhout 0:4141aef83f4b 187
CasperBerkhout 0:4141aef83f4b 188 int main() {
CasperBerkhout 0:4141aef83f4b 189
CasperBerkhout 0:4141aef83f4b 190 //initialize serial comm and set motor PWM freq
CasperBerkhout 0:4141aef83f4b 191 M1_speed.period(1.0/pwm_freq);
CasperBerkhout 0:4141aef83f4b 192 M2_speed.period(1.0/pwm_freq);
CasperBerkhout 0:4141aef83f4b 193 pc.baud(115200);
CasperBerkhout 1:d7299175a12e 194 pc.printf("starting homing function now. Push button to start procedure \n\r");
CasperBerkhout 0:4141aef83f4b 195 //commence homing procedure
CasperBerkhout 0:4141aef83f4b 196 homing_system();
CasperBerkhout 1:d7299175a12e 197 pc.printf("Setting home position complete\n\r");
CasperBerkhout 0:4141aef83f4b 198 //attach all interrupt
CasperBerkhout 1:d7299175a12e 199 pc.printf("attaching interrupt tickers now \n\r");
CasperBerkhout 0:4141aef83f4b 200 button1.fall(StopMotors); //stop motor interrupt
CasperBerkhout 1:d7299175a12e 201 motor_update1.attach(&M1_control,0.01);
CasperBerkhout 1:d7299175a12e 202 motor_update2.attach(&M2_control,0.01);
CasperBerkhout 1:d7299175a12e 203 error_update.attach(&geterror,0.01);
CasperBerkhout 0:4141aef83f4b 204
CasperBerkhout 1:d7299175a12e 205 pc.printf("initialization complete - position control of motors now active\n\r");
CasperBerkhout 0:4141aef83f4b 206
CasperBerkhout 0:4141aef83f4b 207 while(1){
CasperBerkhout 0:4141aef83f4b 208
CasperBerkhout 0:4141aef83f4b 209
CasperBerkhout 0:4141aef83f4b 210 }
CasperBerkhout 0:4141aef83f4b 211
CasperBerkhout 0:4141aef83f4b 212 }