Casper Berkhout / Mbed 2 deprecated BioRobotics_Main

Dependencies:   MODSERIAL QEI biquadFilter mbed

main.cpp@0:4141aef83f4b, 2017-11-01 (annotated)

Committer:
CasperBerkhout
Date:
Wed Nov 01 12:55:50 2017 +0000
Revision:
0:4141aef83f4b
Child:
1:d7299175a12e
Main control structure without EMG

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 0:4141aef83f4b 25 InterruptIn button1(D2); //hardware interrupt for stopping motors
CasperBerkhout 0:4141aef83f4b 26 DigitalIn LimSW1(D1);
CasperBerkhout 0:4141aef83f4b 27 DigitalIn LimSW2(D0);
CasperBerkhout 0:4141aef83f4b 28 DigitalIn HomStart(D3);
CasperBerkhout 0:4141aef83f4b 29 Ticker motor_update;
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 0:4141aef83f4b 33 Ticker scopeTimer;
CasperBerkhout 0:4141aef83f4b 34
CasperBerkhout 0:4141aef83f4b 35
CasperBerkhout 0:4141aef83f4b 36 //-----------------variable decleration----------------
CasperBerkhout 0:4141aef83f4b 37 int pwm_freq = 500;
CasperBerkhout 0:4141aef83f4b 38 float set_speed;
CasperBerkhout 0:4141aef83f4b 39 float reference_pos;
CasperBerkhout 0:4141aef83f4b 40
CasperBerkhout 0:4141aef83f4b 41
CasperBerkhout 0:4141aef83f4b 42 float Arm1_home = 112.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
CasperBerkhout 0:4141aef83f4b 43 float Arm2_home = 19.0/180.0*3.1416;//home position of small link attached to base
CasperBerkhout 0:4141aef83f4b 44
CasperBerkhout 0:4141aef83f4b 45 double M1_home;
CasperBerkhout 0:4141aef83f4b 46 double M1_error_pos = 0;
CasperBerkhout 0:4141aef83f4b 47 float M1_Kp = 0.1;
CasperBerkhout 0:4141aef83f4b 48 float M1_Ki = 0.1;
CasperBerkhout 0:4141aef83f4b 49 float M1_Kd = 0.1;
CasperBerkhout 0:4141aef83f4b 50 double M1_e_int=0;
CasperBerkhout 0:4141aef83f4b 51 double M1_e_prior=0;
CasperBerkhout 0:4141aef83f4b 52
CasperBerkhout 0:4141aef83f4b 53 double M2_home;
CasperBerkhout 0:4141aef83f4b 54 double M2_error_pos = 0;
CasperBerkhout 0:4141aef83f4b 55 float M2_Kp = 0.1;
CasperBerkhout 0:4141aef83f4b 56 float M2_Ki = 0.1;
CasperBerkhout 0:4141aef83f4b 57 float M2_Kd = 0.1;
CasperBerkhout 0:4141aef83f4b 58 double M2_e_int=0;
CasperBerkhout 0:4141aef83f4b 59 double M2_e_prior=0;
CasperBerkhout 0:4141aef83f4b 60
CasperBerkhout 0:4141aef83f4b 61 float Ts = 0.002; //500hz sample freq
CasperBerkhout 0:4141aef83f4b 62
CasperBerkhout 0:4141aef83f4b 63
CasperBerkhout 0:4141aef83f4b 64 void homing_system () {
CasperBerkhout 0:4141aef83f4b 65 LimSW1.mode(PullDown);
CasperBerkhout 0:4141aef83f4b 66 LimSW2.mode(PullDown);
CasperBerkhout 0:4141aef83f4b 67 M1_speed.write(0);
CasperBerkhout 0:4141aef83f4b 68 M2_speed.write(0);
CasperBerkhout 0:4141aef83f4b 69 M1_direction.write(0);
CasperBerkhout 0:4141aef83f4b 70 M2_direction.write(0);
CasperBerkhout 0:4141aef83f4b 71
CasperBerkhout 0:4141aef83f4b 72 while (true){
CasperBerkhout 0:4141aef83f4b 73
CasperBerkhout 0:4141aef83f4b 74 if (HomStart == 0){
CasperBerkhout 0:4141aef83f4b 75 M1_speed.write(0.1);
CasperBerkhout 0:4141aef83f4b 76 }
CasperBerkhout 0:4141aef83f4b 77
CasperBerkhout 0:4141aef83f4b 78 if (LimSW1 == 1){
CasperBerkhout 0:4141aef83f4b 79 M1_speed.write(0);
CasperBerkhout 0:4141aef83f4b 80 M2_speed.write(0.1);
CasperBerkhout 0:4141aef83f4b 81 M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 0:4141aef83f4b 82 }
CasperBerkhout 0:4141aef83f4b 83 if (LimSW2 == 1) {
CasperBerkhout 0:4141aef83f4b 84 M2_speed.write(0);
CasperBerkhout 0:4141aef83f4b 85 M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 0:4141aef83f4b 86 }
CasperBerkhout 0:4141aef83f4b 87
CasperBerkhout 0:4141aef83f4b 88 if (LimSW1 == 1 && LimSW2 ==1) {
CasperBerkhout 0:4141aef83f4b 89 break;
CasperBerkhout 0:4141aef83f4b 90 }
CasperBerkhout 0:4141aef83f4b 91 }
CasperBerkhout 0:4141aef83f4b 92 }
CasperBerkhout 0:4141aef83f4b 93
CasperBerkhout 0:4141aef83f4b 94
CasperBerkhout 0:4141aef83f4b 95 void kinemtaica(double q1, double q2, double vdx, double vdy, double &q1_new, double &q2_new){
CasperBerkhout 0:4141aef83f4b 96 double q1_dot = (2.5*cos(q1+q2))/(sin(q2)) *vdx + (2.5*sin(q1+q2))/(sin(q2))*vdy;
CasperBerkhout 0:4141aef83f4b 97 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 98 q1_new = q1 +q1_dot*Ts;
CasperBerkhout 0:4141aef83f4b 99 q2_new = q2 +q2_dot*Ts;
CasperBerkhout 0:4141aef83f4b 100 return;
CasperBerkhout 0:4141aef83f4b 101 }
CasperBerkhout 0:4141aef83f4b 102
CasperBerkhout 0:4141aef83f4b 103 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 104 e_int += Ts*e;
CasperBerkhout 0:4141aef83f4b 105 double e_diff = (e-e_prior)/Ts;
CasperBerkhout 0:4141aef83f4b 106 e_prior = e;
CasperBerkhout 0:4141aef83f4b 107 double e_diff_filter = bqlowpass.step(e_diff);
CasperBerkhout 0:4141aef83f4b 108 return(Kp*e+Ki*e_int+Kd*e_diff_filter);
CasperBerkhout 0:4141aef83f4b 109 }
CasperBerkhout 0:4141aef83f4b 110
CasperBerkhout 0:4141aef83f4b 111 void M1_control(){
CasperBerkhout 0:4141aef83f4b 112
CasperBerkhout 0:4141aef83f4b 113 //call PID func and set new motor speed
CasperBerkhout 0:4141aef83f4b 114 set_speed = PID(M1_error_pos,M1_Kp,M1_Ki,M1_Kd,Ts,M1_e_int,M1_e_prior);
CasperBerkhout 0:4141aef83f4b 115 if(set_speed > 0){
CasperBerkhout 0:4141aef83f4b 116 M1_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 117 M1_direction.write(0);
CasperBerkhout 0:4141aef83f4b 118 }
CasperBerkhout 0:4141aef83f4b 119 else if (set_speed < 0){
CasperBerkhout 0:4141aef83f4b 120 M1_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 121 M1_direction.write(1);
CasperBerkhout 0:4141aef83f4b 122 }
CasperBerkhout 0:4141aef83f4b 123 else{M1_speed.write(0);}
CasperBerkhout 0:4141aef83f4b 124 }
CasperBerkhout 0:4141aef83f4b 125
CasperBerkhout 0:4141aef83f4b 126 void M2_control(){
CasperBerkhout 0:4141aef83f4b 127 set_speed = PID(M2_error_pos,M2_Kp,M2_Ki,M2_Kd,Ts,M2_e_int,M2_e_prior);
CasperBerkhout 0:4141aef83f4b 128 if(set_speed > 0){
CasperBerkhout 0:4141aef83f4b 129 M2_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 130 M2_direction.write(0);
CasperBerkhout 0:4141aef83f4b 131 }
CasperBerkhout 0:4141aef83f4b 132 else if (set_speed < 0){
CasperBerkhout 0:4141aef83f4b 133 M2_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 134 M2_direction.write(1);
CasperBerkhout 0:4141aef83f4b 135 }
CasperBerkhout 0:4141aef83f4b 136 else{M2_speed.write(0);}
CasperBerkhout 0:4141aef83f4b 137 }
CasperBerkhout 0:4141aef83f4b 138
CasperBerkhout 0:4141aef83f4b 139
CasperBerkhout 0:4141aef83f4b 140
CasperBerkhout 0:4141aef83f4b 141 void scopesend(){
CasperBerkhout 0:4141aef83f4b 142
CasperBerkhout 0:4141aef83f4b 143
CasperBerkhout 0:4141aef83f4b 144
CasperBerkhout 0:4141aef83f4b 145 }
CasperBerkhout 0:4141aef83f4b 146 void StopMotors(){
CasperBerkhout 0:4141aef83f4b 147 while(1){
CasperBerkhout 0:4141aef83f4b 148 M1_speed.write(0);
CasperBerkhout 0:4141aef83f4b 149 M2_speed.write(0);
CasperBerkhout 0:4141aef83f4b 150 }
CasperBerkhout 0:4141aef83f4b 151 }
CasperBerkhout 0:4141aef83f4b 152
CasperBerkhout 0:4141aef83f4b 153 void geterror(){
CasperBerkhout 0:4141aef83f4b 154 double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
CasperBerkhout 0:4141aef83f4b 155 double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
CasperBerkhout 0:4141aef83f4b 156
CasperBerkhout 0:4141aef83f4b 157 double M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!!
CasperBerkhout 0:4141aef83f4b 158 double M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!!
CasperBerkhout 0:4141aef83f4b 159
CasperBerkhout 0:4141aef83f4b 160 double q1 = M1_actual_pos;
CasperBerkhout 0:4141aef83f4b 161 double q2 = 3.1416-M1_acual_pos-M2_actual_pos //see drawing
CasperBerkhout 0:4141aef83f4b 162
CasperBerkhout 0:4141aef83f4b 163 double M1_reference_pos = potmeter.read();
CasperBerkhout 0:4141aef83f4b 164 double M2_reference_pos = potmeter2.read();
CasperBerkhout 0:4141aef83f4b 165
CasperBerkhout 0:4141aef83f4b 166 M1_error_pos = M1_reference_pos - M1_actual_pos;
CasperBerkhout 0:4141aef83f4b 167 M2_error_pos = M2_reference_pos - M2_actual_pos;
CasperBerkhout 0:4141aef83f4b 168 }
CasperBerkhout 0:4141aef83f4b 169
CasperBerkhout 0:4141aef83f4b 170 int main() {
CasperBerkhout 0:4141aef83f4b 171
CasperBerkhout 0:4141aef83f4b 172 //initialize serial comm and set motor PWM freq
CasperBerkhout 0:4141aef83f4b 173 M1_speed.period(1.0/pwm_freq);
CasperBerkhout 0:4141aef83f4b 174 M2_speed.period(1.0/pwm_freq);
CasperBerkhout 0:4141aef83f4b 175 pc.baud(115200);
CasperBerkhout 0:4141aef83f4b 176 //commence homing procedure
CasperBerkhout 0:4141aef83f4b 177 homing_system();
CasperBerkhout 0:4141aef83f4b 178 //attach all interrupt
CasperBerkhout 0:4141aef83f4b 179 button1.fall(StopMotors); //stop motor interrupt
CasperBerkhout 0:4141aef83f4b 180 motor_update.attach(&M1_control, &M2_control,0.01);
CasperBerkhout 0:4141aef83f4b 181 //motor_update.attach(&M2_control,0.01);
CasperBerkhout 0:4141aef83f4b 182
CasperBerkhout 0:4141aef83f4b 183
CasperBerkhout 0:4141aef83f4b 184
CasperBerkhout 0:4141aef83f4b 185
CasperBerkhout 0:4141aef83f4b 186
CasperBerkhout 0:4141aef83f4b 187
CasperBerkhout 0:4141aef83f4b 188 pc.printf("initialization complete \n\r");
CasperBerkhout 0:4141aef83f4b 189
CasperBerkhout 0:4141aef83f4b 190 while(1){
CasperBerkhout 0:4141aef83f4b 191
CasperBerkhout 0:4141aef83f4b 192
CasperBerkhout 0:4141aef83f4b 193 }
CasperBerkhout 0:4141aef83f4b 194
CasperBerkhout 0:4141aef83f4b 195 }