maaike laagland / Mbed 2 deprecated BioRobotics_Main

hoi

Dependencies:   mbed QEI biquadFilter MODSERIAL

main.cpp@9:5023f5a21eab, 2017-11-02 (annotated)

Committer:
CasperBerkhout
Date:
Thu Nov 02 19:48:09 2017 +0000
Revision:
9:5023f5a21eab
Parent:
8:d608c797ea2c
Child:
10:e328acbdf885
Intergrated output from kinematics

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 4:de0923cf6bcc 30 BiQuad bqlowpass(0.4354, 0.8709, 0.4354, 0.5179, 0.2238);
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 double M1_home;
CasperBerkhout 0:4141aef83f4b 45 double M1_error_pos = 0;
CasperBerkhout 9:5023f5a21eab 46 float M1_Kp = 2.2;
CasperBerkhout 9:5023f5a21eab 47 float M1_Ki = 3.8;
CasperBerkhout 3:455b79d42636 48 float M1_Kd = 0.19;
CasperBerkhout 0:4141aef83f4b 49 double M1_e_int=0;
CasperBerkhout 0:4141aef83f4b 50 double M1_e_prior=0;
CasperBerkhout 0:4141aef83f4b 51
CasperBerkhout 0:4141aef83f4b 52 double M2_home;
CasperBerkhout 0:4141aef83f4b 53 double M2_error_pos = 0;
CasperBerkhout 8:d608c797ea2c 54 float M2_Kp = 2;
CasperBerkhout 6:b0b15eb27de1 55 float M2_Ki = 2.5;
CasperBerkhout 9:5023f5a21eab 56 float M2_Kd = 0.1;
CasperBerkhout 0:4141aef83f4b 57 double M2_e_int=0;
CasperBerkhout 0:4141aef83f4b 58 double M2_e_prior=0;
CasperBerkhout 0:4141aef83f4b 59
CasperBerkhout 2:c7369b41f7ae 60 double setpoint = 0;
CasperBerkhout 2:c7369b41f7ae 61
CasperBerkhout 2:c7369b41f7ae 62 double M1_rel_pos;
CasperBerkhout 2:c7369b41f7ae 63 double M2_rel_pos;
CasperBerkhout 9:5023f5a21eab 64 double M1_reference_pos;
CasperBerkhout 5:9e8847a0492c 65 double M2_reference_pos;
CasperBerkhout 2:c7369b41f7ae 66
CasperBerkhout 6:b0b15eb27de1 67 double q1_step;
CasperBerkhout 6:b0b15eb27de1 68 double q2_step;
CasperBerkhout 9:5023f5a21eab 69 double Arm1_home;
CasperBerkhout 9:5023f5a21eab 70 double Arm2_home;
CasperBerkhout 9:5023f5a21eab 71 double M1_actual_pos;
CasperBerkhout 9:5023f5a21eab 72 double M2_actual_pos;
CasperBerkhout 9:5023f5a21eab 73 double q1;
CasperBerkhout 9:5023f5a21eab 74 double q2;
CasperBerkhout 9:5023f5a21eab 75 double vdx;
CasperBerkhout 9:5023f5a21eab 76 double vdy;
CasperBerkhout 9:5023f5a21eab 77
CasperBerkhout 6:b0b15eb27de1 78
CasperBerkhout 6:b0b15eb27de1 79 float Ts = 0.01; //500hz sample freq
CasperBerkhout 0:4141aef83f4b 80
CasperBerkhout 1:d7299175a12e 81 bool M1homflag;
CasperBerkhout 1:d7299175a12e 82 bool M2homflag;
CasperBerkhout 1:d7299175a12e 83 bool Homstartflag;
CasperBerkhout 1:d7299175a12e 84
CasperBerkhout 1:d7299175a12e 85
CasperBerkhout 0:4141aef83f4b 86
CasperBerkhout 0:4141aef83f4b 87
CasperBerkhout 6:b0b15eb27de1 88 void kinematica(double q1, double q2, double vdx, double vdy, double &q1_new, double &q2_new){
CasperBerkhout 0:4141aef83f4b 89 double q1_dot = (2.5*cos(q1+q2))/(sin(q2)) *vdx + (2.5*sin(q1+q2))/(sin(q2))*vdy;
CasperBerkhout 7:c78b25ef3c7b 90 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(q1))/(0.12*sin(q2))*vdy;
CasperBerkhout 9:5023f5a21eab 91 q1_new += q1_dot*Ts;
CasperBerkhout 9:5023f5a21eab 92 q2_new += (q2_dot-q1_dot)*Ts;
CasperBerkhout 9:5023f5a21eab 93 //pc.printf("q1 dot = %f, q2 dot = %f, ",q1_dot,q2_dot);
CasperBerkhout 0:4141aef83f4b 94 return;
CasperBerkhout 0:4141aef83f4b 95 }
CasperBerkhout 0:4141aef83f4b 96
CasperBerkhout 0:4141aef83f4b 97 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 98 e_int += Ts*e;
CasperBerkhout 0:4141aef83f4b 99 double e_diff = (e-e_prior)/Ts;
CasperBerkhout 0:4141aef83f4b 100 e_prior = e;
CasperBerkhout 0:4141aef83f4b 101 double e_diff_filter = bqlowpass.step(e_diff);
CasperBerkhout 0:4141aef83f4b 102 return(Kp*e+Ki*e_int+Kd*e_diff_filter);
CasperBerkhout 0:4141aef83f4b 103 }
CasperBerkhout 0:4141aef83f4b 104
CasperBerkhout 0:4141aef83f4b 105 void M1_control(){
CasperBerkhout 0:4141aef83f4b 106
CasperBerkhout 0:4141aef83f4b 107 //call PID func and set new motor speed
CasperBerkhout 0:4141aef83f4b 108 set_speed = PID(M1_error_pos,M1_Kp,M1_Ki,M1_Kd,Ts,M1_e_int,M1_e_prior);
CasperBerkhout 0:4141aef83f4b 109 if(set_speed > 0){
CasperBerkhout 0:4141aef83f4b 110 M1_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 111 M1_direction.write(0);
CasperBerkhout 0:4141aef83f4b 112 }
CasperBerkhout 0:4141aef83f4b 113 else if (set_speed < 0){
CasperBerkhout 0:4141aef83f4b 114 M1_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 115 M1_direction.write(1);
CasperBerkhout 0:4141aef83f4b 116 }
CasperBerkhout 0:4141aef83f4b 117 else{M1_speed.write(0);}
CasperBerkhout 4:de0923cf6bcc 118
CasperBerkhout 0:4141aef83f4b 119 }
CasperBerkhout 0:4141aef83f4b 120
CasperBerkhout 0:4141aef83f4b 121 void M2_control(){
CasperBerkhout 0:4141aef83f4b 122 set_speed = PID(M2_error_pos,M2_Kp,M2_Ki,M2_Kd,Ts,M2_e_int,M2_e_prior);
CasperBerkhout 0:4141aef83f4b 123 if(set_speed > 0){
CasperBerkhout 0:4141aef83f4b 124 M2_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 125 M2_direction.write(0);
CasperBerkhout 0:4141aef83f4b 126 }
CasperBerkhout 0:4141aef83f4b 127 else if (set_speed < 0){
CasperBerkhout 0:4141aef83f4b 128 M2_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 129 M2_direction.write(1);
CasperBerkhout 0:4141aef83f4b 130 }
CasperBerkhout 0:4141aef83f4b 131 else{M2_speed.write(0);}
CasperBerkhout 7:c78b25ef3c7b 132 //pc.printf("M2 speed = %f, M2 direction = %i, M2 pos error = %f, M2 int error = %f\n\r",set_speed,M2_direction.read(),M2_error_pos,M2_e_int);
CasperBerkhout 5:9e8847a0492c 133 //pc.printf("M2 integral = %f position error = %f\n\r", M2_e_int,M2_error_pos);
CasperBerkhout 0:4141aef83f4b 134 }
CasperBerkhout 0:4141aef83f4b 135
CasperBerkhout 2:c7369b41f7ae 136 void homing_system () {
CasperBerkhout 2:c7369b41f7ae 137 LimSW1.mode(PullDown);
CasperBerkhout 2:c7369b41f7ae 138 LimSW2.mode(PullDown);
CasperBerkhout 2:c7369b41f7ae 139 M1_speed.write(0);
CasperBerkhout 2:c7369b41f7ae 140 M2_speed.write(0);
CasperBerkhout 5:9e8847a0492c 141
CasperBerkhout 2:c7369b41f7ae 142
CasperBerkhout 2:c7369b41f7ae 143 while(1){
CasperBerkhout 3:455b79d42636 144 if (HomStart.read() == 0){
CasperBerkhout 6:b0b15eb27de1 145 setpoint += 0.005; //move setpoint 0.2 radian per second (at 100hz)
CasperBerkhout 7:c78b25ef3c7b 146 //pc.printf("Homing... \n\r");
CasperBerkhout 4:de0923cf6bcc 147 }
CasperBerkhout 4:de0923cf6bcc 148
CasperBerkhout 4:de0923cf6bcc 149 double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416;
CasperBerkhout 4:de0923cf6bcc 150 double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416;
CasperBerkhout 3:455b79d42636 151 if(LimSW1.read() == 0){
CasperBerkhout 7:c78b25ef3c7b 152 M1_error_pos = setpoint - M1_rel_pos;
CasperBerkhout 7:c78b25ef3c7b 153 M1_control();
CasperBerkhout 3:455b79d42636 154 }
CasperBerkhout 3:455b79d42636 155 if(LimSW2.read() == 0){
CasperBerkhout 5:9e8847a0492c 156 M2_error_pos = - setpoint - M2_rel_pos;
CasperBerkhout 7:c78b25ef3c7b 157 M2_control();
CasperBerkhout 3:455b79d42636 158 }
CasperBerkhout 7:c78b25ef3c7b 159
CasperBerkhout 7:c78b25ef3c7b 160
CasperBerkhout 4:de0923cf6bcc 161
CasperBerkhout 4:de0923cf6bcc 162
CasperBerkhout 2:c7369b41f7ae 163 if(LimSW1.read() == 1){
CasperBerkhout 2:c7369b41f7ae 164 M1_error_pos = 0;
CasperBerkhout 2:c7369b41f7ae 165 M1_speed.write(0);
CasperBerkhout 2:c7369b41f7ae 166 M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 2:c7369b41f7ae 167 }
CasperBerkhout 2:c7369b41f7ae 168 if (LimSW2.read() == 1) {
CasperBerkhout 2:c7369b41f7ae 169 M2_error_pos = 0;
CasperBerkhout 2:c7369b41f7ae 170 M2_speed.write(0);
CasperBerkhout 2:c7369b41f7ae 171 M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 2:c7369b41f7ae 172 }
CasperBerkhout 2:c7369b41f7ae 173
CasperBerkhout 2:c7369b41f7ae 174 if (LimSW1.read() == 1 && LimSW2.read() ==1) {
CasperBerkhout 2:c7369b41f7ae 175 pc.printf("Homing finished \n\r");
CasperBerkhout 2:c7369b41f7ae 176 M1_speed.write(0);
CasperBerkhout 2:c7369b41f7ae 177 M2_speed.write(0);
CasperBerkhout 7:c78b25ef3c7b 178 wait(0.1);
CasperBerkhout 2:c7369b41f7ae 179 M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 2:c7369b41f7ae 180 M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 7:c78b25ef3c7b 181 M1_e_int = 0;
CasperBerkhout 7:c78b25ef3c7b 182 M2_e_int = 0;
CasperBerkhout 5:9e8847a0492c 183 break;
CasperBerkhout 5:9e8847a0492c 184 //while(1); //stop after homing.
CasperBerkhout 2:c7369b41f7ae 185 }
CasperBerkhout 8:d608c797ea2c 186 pc.printf("M2 error = %f M2 reference = %f,M2_rel = %f\n\r", M2_error_pos,M2_reference_pos,M2_rel_pos);
CasperBerkhout 4:de0923cf6bcc 187 wait(0.01);
CasperBerkhout 2:c7369b41f7ae 188 }
CasperBerkhout 0:4141aef83f4b 189
CasperBerkhout 2:c7369b41f7ae 190 }
CasperBerkhout 0:4141aef83f4b 191
CasperBerkhout 0:4141aef83f4b 192 void scopesend(){
CasperBerkhout 0:4141aef83f4b 193
CasperBerkhout 0:4141aef83f4b 194
CasperBerkhout 0:4141aef83f4b 195
CasperBerkhout 0:4141aef83f4b 196 }
CasperBerkhout 0:4141aef83f4b 197 void StopMotors(){
CasperBerkhout 0:4141aef83f4b 198 while(1){
CasperBerkhout 0:4141aef83f4b 199 M1_speed.write(0);
CasperBerkhout 0:4141aef83f4b 200 M2_speed.write(0);
CasperBerkhout 0:4141aef83f4b 201 }
CasperBerkhout 0:4141aef83f4b 202 }
CasperBerkhout 0:4141aef83f4b 203
CasperBerkhout 0:4141aef83f4b 204 void geterror(){
CasperBerkhout 9:5023f5a21eab 205 M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
CasperBerkhout 9:5023f5a21eab 206 M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
CasperBerkhout 2:c7369b41f7ae 207
CasperBerkhout 9:5023f5a21eab 208 Arm1_home = 132.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
CasperBerkhout 9:5023f5a21eab 209 Arm2_home = 5.0/180.0*3.1416;//home position of small link attached to base
CasperBerkhout 0:4141aef83f4b 210
CasperBerkhout 9:5023f5a21eab 211 M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle theta
CasperBerkhout 9:5023f5a21eab 212 M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle Alpha
CasperBerkhout 0:4141aef83f4b 213
CasperBerkhout 9:5023f5a21eab 214 q1 = M1_actual_pos;
CasperBerkhout 9:5023f5a21eab 215 q2 = -q1 - M2_actual_pos; //see drawing
CasperBerkhout 6:b0b15eb27de1 216
CasperBerkhout 9:5023f5a21eab 217 vdx = 0.1*potmeter.read();
CasperBerkhout 9:5023f5a21eab 218 vdy = -0.1*potmeter2.read();
CasperBerkhout 0:4141aef83f4b 219
CasperBerkhout 6:b0b15eb27de1 220 kinematica(q1,q2,vdx,vdy,q1_step,q2_step);
CasperBerkhout 6:b0b15eb27de1 221
CasperBerkhout 9:5023f5a21eab 222 M1_reference_pos = q1_step; //should cover the right range - radians
CasperBerkhout 9:5023f5a21eab 223 M2_reference_pos = -(q1_step+q2_step);
CasperBerkhout 6:b0b15eb27de1 224
CasperBerkhout 6:b0b15eb27de1 225
CasperBerkhout 6:b0b15eb27de1 226
CasperBerkhout 0:4141aef83f4b 227
CasperBerkhout 3:455b79d42636 228
CasperBerkhout 9:5023f5a21eab 229 pc.printf("VDX = %f, q1 = %f, q2 = %f,q1_step = %f, q2_step = %f,M1_rel = %f, M2_rel = %f, M2_reference = %f, M1_error_pos = %f, M2_error_pos = %f\n\r",vdx, q1, q2,q1_step,q2_step,M1_rel_pos,M2_rel_pos, M2_reference_pos,M1_error_pos,M2_error_pos);
CasperBerkhout 6:b0b15eb27de1 230
CasperBerkhout 3:455b79d42636 231 if(M1_reference_pos > Arm1_home){
CasperBerkhout 3:455b79d42636 232 M1_reference_pos = Arm1_home;
CasperBerkhout 3:455b79d42636 233 }
CasperBerkhout 3:455b79d42636 234 else{
CasperBerkhout 3:455b79d42636 235 M1_error_pos = M1_reference_pos - M1_actual_pos;
CasperBerkhout 3:455b79d42636 236 }
CasperBerkhout 5:9e8847a0492c 237 if(M2_reference_pos < Arm2_home){
CasperBerkhout 3:455b79d42636 238 M2_reference_pos = Arm2_home;
CasperBerkhout 3:455b79d42636 239 }
CasperBerkhout 3:455b79d42636 240 else{
CasperBerkhout 3:455b79d42636 241 M2_error_pos = M2_reference_pos - M2_actual_pos;
CasperBerkhout 3:455b79d42636 242 }
CasperBerkhout 9:5023f5a21eab 243
CasperBerkhout 9:5023f5a21eab 244
CasperBerkhout 9:5023f5a21eab 245 //---------PID motor control-------------
CasperBerkhout 9:5023f5a21eab 246
CasperBerkhout 9:5023f5a21eab 247 set_speed = PID(M1_error_pos,M1_Kp,M1_Ki,M1_Kd,Ts,M1_e_int,M1_e_prior);
CasperBerkhout 9:5023f5a21eab 248 if(set_speed > 0){
CasperBerkhout 9:5023f5a21eab 249 M1_speed.write(abs(set_speed));
CasperBerkhout 9:5023f5a21eab 250 M1_direction.write(0);
CasperBerkhout 9:5023f5a21eab 251 }
CasperBerkhout 9:5023f5a21eab 252 else if (set_speed < 0){
CasperBerkhout 9:5023f5a21eab 253 M1_speed.write(abs(set_speed));
CasperBerkhout 9:5023f5a21eab 254 M1_direction.write(1);
CasperBerkhout 9:5023f5a21eab 255 }
CasperBerkhout 9:5023f5a21eab 256 else{M1_speed.write(0);}
CasperBerkhout 9:5023f5a21eab 257 set_speed = PID(M2_error_pos,M2_Kp,M2_Ki,M2_Kd,Ts,M2_e_int,M2_e_prior);
CasperBerkhout 9:5023f5a21eab 258 if(set_speed > 0){
CasperBerkhout 9:5023f5a21eab 259 M2_speed.write(abs(set_speed));
CasperBerkhout 9:5023f5a21eab 260 M2_direction.write(0);
CasperBerkhout 9:5023f5a21eab 261 }
CasperBerkhout 9:5023f5a21eab 262 else if (set_speed < 0){
CasperBerkhout 9:5023f5a21eab 263 M2_speed.write(abs(set_speed));
CasperBerkhout 9:5023f5a21eab 264 M2_direction.write(1);
CasperBerkhout 9:5023f5a21eab 265 }
CasperBerkhout 9:5023f5a21eab 266 else{M2_speed.write(0);}
CasperBerkhout 0:4141aef83f4b 267 }
CasperBerkhout 0:4141aef83f4b 268
CasperBerkhout 0:4141aef83f4b 269 int main() {
CasperBerkhout 5:9e8847a0492c 270 button1.fall(StopMotors);
CasperBerkhout 0:4141aef83f4b 271 //initialize serial comm and set motor PWM freq
CasperBerkhout 0:4141aef83f4b 272 M1_speed.period(1.0/pwm_freq);
CasperBerkhout 0:4141aef83f4b 273 M2_speed.period(1.0/pwm_freq);
CasperBerkhout 0:4141aef83f4b 274 pc.baud(115200);
CasperBerkhout 1:d7299175a12e 275 pc.printf("starting homing function now. Push button to start procedure \n\r");
CasperBerkhout 0:4141aef83f4b 276 //commence homing procedure
CasperBerkhout 0:4141aef83f4b 277 homing_system();
CasperBerkhout 1:d7299175a12e 278 pc.printf("Setting home position complete\n\r");
CasperBerkhout 0:4141aef83f4b 279 //attach all interrupt
CasperBerkhout 1:d7299175a12e 280 pc.printf("attaching interrupt tickers now \n\r");
CasperBerkhout 9:5023f5a21eab 281 M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
CasperBerkhout 9:5023f5a21eab 282 M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
CasperBerkhout 9:5023f5a21eab 283
CasperBerkhout 9:5023f5a21eab 284 Arm1_home = 132.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
CasperBerkhout 9:5023f5a21eab 285 Arm2_home = 5.0/180.0*3.1416;//home position of small link attached to base
CasperBerkhout 9:5023f5a21eab 286
CasperBerkhout 9:5023f5a21eab 287 M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle theta
CasperBerkhout 9:5023f5a21eab 288 M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle Alpha
CasperBerkhout 9:5023f5a21eab 289
CasperBerkhout 9:5023f5a21eab 290 q1 = M1_actual_pos;
CasperBerkhout 9:5023f5a21eab 291 q2 = -q1 - M2_actual_pos; //see drawing
CasperBerkhout 9:5023f5a21eab 292
CasperBerkhout 9:5023f5a21eab 293 q1_step = q1;
CasperBerkhout 9:5023f5a21eab 294 q2_step = q2;
CasperBerkhout 5:9e8847a0492c 295 //stop motor interrupt
CasperBerkhout 9:5023f5a21eab 296 //motor_update1.attach(&M1_control,0.01);
CasperBerkhout 9:5023f5a21eab 297 //motor_update2.attach(&M2_control,0.01);
CasperBerkhout 9:5023f5a21eab 298 error_update.attach(&geterror,0.02);
CasperBerkhout 0:4141aef83f4b 299
CasperBerkhout 1:d7299175a12e 300 pc.printf("initialization complete - position control of motors now active\n\r");
CasperBerkhout 0:4141aef83f4b 301
CasperBerkhout 0:4141aef83f4b 302 while(1){
CasperBerkhout 9:5023f5a21eab 303
CasperBerkhout 0:4141aef83f4b 304
CasperBerkhout 0:4141aef83f4b 305 }
CasperBerkhout 0:4141aef83f4b 306
CasperBerkhout 0:4141aef83f4b 307 }