RobotArm_ Biorobotics project.
Dependencies: PID QEI RemoteIR Servo mbed
Fork of Biorobotics_Motor_Control by
main.cpp@5:74784836db3d, 2017-10-27 (annotated)
- Committer:
- jordymorsinkhof
- Date:
- Fri Oct 27 11:47:47 2017 +0000
- Revision:
- 5:74784836db3d
- Parent:
- 4:c45eaa904b09
Wordt op dit moment aangestuurd in Xrichting en in Yrichting;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
BramS23 | 0:6c0f87177797 | 1 | #include "mbed.h" |
BramS23 | 0:6c0f87177797 | 2 | #include "QEI.h" |
jordymorsinkhof | 2:684d5cf2f683 | 3 | #include "PID.h" |
jordymorsinkhof | 2:684d5cf2f683 | 4 | #include "ReceiverIR.h" |
jordymorsinkhof | 2:684d5cf2f683 | 5 | #include "Servo.h" |
jordymorsinkhof | 3:98ea6afa0cf2 | 6 | #include <iostream> |
jordymorsinkhof | 3:98ea6afa0cf2 | 7 | #include <string> |
jordymorsinkhof | 3:98ea6afa0cf2 | 8 | #include <math.h> |
jordymorsinkhof | 2:684d5cf2f683 | 9 | |
jordymorsinkhof | 2:684d5cf2f683 | 10 | ReceiverIR ir_rx(D2); |
jordymorsinkhof | 2:684d5cf2f683 | 11 | |
jordymorsinkhof | 2:684d5cf2f683 | 12 | AnalogIn PotMeter1(A0); |
jordymorsinkhof | 2:684d5cf2f683 | 13 | AnalogIn PotMeter2(A1); |
BramS23 | 0:6c0f87177797 | 14 | InterruptIn Button(PTA4); |
BramS23 | 0:6c0f87177797 | 15 | |
BramS23 | 0:6c0f87177797 | 16 | PwmOut MotorThrottle1(D5); |
BramS23 | 0:6c0f87177797 | 17 | PwmOut MotorThrottle2(D6); |
BramS23 | 0:6c0f87177797 | 18 | DigitalOut MotorDirection1(D4); |
BramS23 | 0:6c0f87177797 | 19 | DigitalOut MotorDirection2(D7); |
jordymorsinkhof | 2:684d5cf2f683 | 20 | DigitalOut servo(D3); |
BramS23 | 0:6c0f87177797 | 21 | |
jordymorsinkhof | 2:684d5cf2f683 | 22 | DigitalIn EndSwitch1(D9); |
jordymorsinkhof | 2:684d5cf2f683 | 23 | DigitalIn EndSwitch2(D8); |
jordymorsinkhof | 2:684d5cf2f683 | 24 | |
jordymorsinkhof | 2:684d5cf2f683 | 25 | QEI EncoderMotor1(D12,D13,NC,4200); |
jordymorsinkhof | 2:684d5cf2f683 | 26 | QEI EncoderMotor2(D10,D11,NC,4200); |
jordymorsinkhof | 2:684d5cf2f683 | 27 | |
jordymorsinkhof | 2:684d5cf2f683 | 28 | Ticker ControlTicker; |
jordymorsinkhof | 2:684d5cf2f683 | 29 | Ticker GetRefTicker; |
jordymorsinkhof | 2:684d5cf2f683 | 30 | |
jordymorsinkhof | 2:684d5cf2f683 | 31 | float Interval=0.02f; |
jordymorsinkhof | 3:98ea6afa0cf2 | 32 | float pi=3.14159265359; |
jordymorsinkhof | 3:98ea6afa0cf2 | 33 | |
jordymorsinkhof | 4:c45eaa904b09 | 34 | PID controller1(100.0f,0.0f,0.001f, Interval); |
jordymorsinkhof | 4:c45eaa904b09 | 35 | PID controller2(100.0f,0.0f,0.002f, Interval); |
jordymorsinkhof | 2:684d5cf2f683 | 36 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 37 | //G: elsewhere given values |
jordymorsinkhof | 4:c45eaa904b09 | 38 | float GXset = 40.5; //from EMG in cm |
jordymorsinkhof | 3:98ea6afa0cf2 | 39 | float GYset = 11; //from EMG in cm |
jordymorsinkhof | 3:98ea6afa0cf2 | 40 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 41 | //Constant robot parameters |
jordymorsinkhof | 3:98ea6afa0cf2 | 42 | const float L1 = 27.5; //length arm 1 in cm |
jordymorsinkhof | 3:98ea6afa0cf2 | 43 | const float L2 = 32; //length arm 2 in cm |
jordymorsinkhof | 3:98ea6afa0cf2 | 44 | |
jordymorsinkhof | 4:c45eaa904b09 | 45 | //Motor angles in radialen |
jordymorsinkhof | 4:c45eaa904b09 | 46 | float q1set = 0.25f*pi; |
jordymorsinkhof | 4:c45eaa904b09 | 47 | float q2set = -0.5f*pi; |
jordymorsinkhof | 3:98ea6afa0cf2 | 48 | |
jordymorsinkhof | 2:684d5cf2f683 | 49 | RemoteIR::Format format; |
jordymorsinkhof | 2:684d5cf2f683 | 50 | uint8_t buf[4]; |
BramS23 | 0:6c0f87177797 | 51 | |
jordymorsinkhof | 4:c45eaa904b09 | 52 | RawSerial pc(PTB17,PTB16); |
jordymorsinkhof | 2:684d5cf2f683 | 53 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 54 | //Function INPUT: q1, q2. OUTPUT: Xcurr, Ycurr |
jordymorsinkhof | 3:98ea6afa0cf2 | 55 | void Brock(float q1, float q2, float& Xcurr, float& Ycurr) |
jordymorsinkhof | 3:98ea6afa0cf2 | 56 | { |
jordymorsinkhof | 3:98ea6afa0cf2 | 57 | //Position of end-effector in base frame when q1 = q2 = 0 to use in He0(0) |
jordymorsinkhof | 3:98ea6afa0cf2 | 58 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 59 | float He1[9]; |
jordymorsinkhof | 3:98ea6afa0cf2 | 60 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 61 | He1[0] = cos(q1 + q2); |
jordymorsinkhof | 3:98ea6afa0cf2 | 62 | He1[1] = -sin(q1 +q2); |
jordymorsinkhof | 3:98ea6afa0cf2 | 63 | He1[2] = L2*cos(q1 + q2) + L1*cos(q1); |
jordymorsinkhof | 3:98ea6afa0cf2 | 64 | He1[3] = sin(q1 + q2); |
jordymorsinkhof | 3:98ea6afa0cf2 | 65 | He1[4] = cos(q1 + q2); |
jordymorsinkhof | 3:98ea6afa0cf2 | 66 | He1[5] = L2*sin(q1 + q2) + L1*sin(q1); |
jordymorsinkhof | 3:98ea6afa0cf2 | 67 | He1[6] = 0; |
jordymorsinkhof | 3:98ea6afa0cf2 | 68 | He1[7] = 0; |
jordymorsinkhof | 3:98ea6afa0cf2 | 69 | He1[8] = 1; |
jordymorsinkhof | 3:98ea6afa0cf2 | 70 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 71 | // print He1 to check the matrix |
jordymorsinkhof | 4:c45eaa904b09 | 72 | /* |
jordymorsinkhof | 3:98ea6afa0cf2 | 73 | pc.printf("He1 = [\n\r"); |
jordymorsinkhof | 3:98ea6afa0cf2 | 74 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 75 | for (int i=0; i<=8; i++){ |
jordymorsinkhof | 2:684d5cf2f683 | 76 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 77 | pc.printf("%.2f ",He1[i]); |
jordymorsinkhof | 3:98ea6afa0cf2 | 78 | if (i==2){ |
jordymorsinkhof | 3:98ea6afa0cf2 | 79 | pc.printf("\n\r");} |
jordymorsinkhof | 3:98ea6afa0cf2 | 80 | else if (i==5){ |
jordymorsinkhof | 3:98ea6afa0cf2 | 81 | pc.printf("\n\r");} |
jordymorsinkhof | 3:98ea6afa0cf2 | 82 | } |
jordymorsinkhof | 3:98ea6afa0cf2 | 83 | pc.printf("]\n\r" ); |
jordymorsinkhof | 4:c45eaa904b09 | 84 | */ |
jordymorsinkhof | 3:98ea6afa0cf2 | 85 | //Translation from base frame to board frame SO FROM NOW ON ALL EXPRESSED IN BOARD FRAME |
jordymorsinkhof | 4:c45eaa904b09 | 86 | He1[2] = He1[2] - 14.2; //12 = x distance from base frame to board frame |
jordymorsinkhof | 4:c45eaa904b09 | 87 | He1[5] = He1[5] + 11.9; //11 = y distance from base frame to board frame |
jordymorsinkhof | 3:98ea6afa0cf2 | 88 | |
jordymorsinkhof | 4:c45eaa904b09 | 89 | //pc.printf("The old value of Xcurr was %f, old Ycurr was %f\n\r", Xcurr, Ycurr); |
jordymorsinkhof | 3:98ea6afa0cf2 | 90 | Xcurr = He1[2]; |
jordymorsinkhof | 3:98ea6afa0cf2 | 91 | Ycurr = He1[5]; |
jordymorsinkhof | 4:c45eaa904b09 | 92 | //pc.printf("The new value of Xcurr is %f, new Ycurr is %f\n\r", Xcurr, Ycurr); |
jordymorsinkhof | 4:c45eaa904b09 | 93 | /* |
jordymorsinkhof | 3:98ea6afa0cf2 | 94 | // print He0 to check the matrix |
jordymorsinkhof | 3:98ea6afa0cf2 | 95 | pc.printf("\n\r He0 = [\n\r"); |
jordymorsinkhof | 3:98ea6afa0cf2 | 96 | for (int i=0; i<=8; i++){ |
jordymorsinkhof | 3:98ea6afa0cf2 | 97 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 98 | pc.printf("%.2f ",He1[i]); |
jordymorsinkhof | 3:98ea6afa0cf2 | 99 | if (i==2){ |
jordymorsinkhof | 3:98ea6afa0cf2 | 100 | pc.printf("\n\r");} |
jordymorsinkhof | 3:98ea6afa0cf2 | 101 | else if (i==5){ |
jordymorsinkhof | 3:98ea6afa0cf2 | 102 | pc.printf("\n\r");} |
jordymorsinkhof | 3:98ea6afa0cf2 | 103 | } |
jordymorsinkhof | 3:98ea6afa0cf2 | 104 | pc.printf("]\n\r" ); |
jordymorsinkhof | 4:c45eaa904b09 | 105 | */ |
jordymorsinkhof | 3:98ea6afa0cf2 | 106 | } |
jordymorsinkhof | 3:98ea6afa0cf2 | 107 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 108 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 109 | //Function INPUT: q1, q2, Xset, Yset, Xcurr, Ycurr. OUTPUT: tau1, tau2 |
jordymorsinkhof | 3:98ea6afa0cf2 | 110 | void ErrorToTau(float q1, float q2, float Xcurr, float Ycurr, float Xset, float Yset, float& tau1, float& tau2) |
jordymorsinkhof | 3:98ea6afa0cf2 | 111 | { |
jordymorsinkhof | 3:98ea6afa0cf2 | 112 | //The parameters k and b are free controller parameters that will determine how fast the arm moves towards the setpoint. |
jordymorsinkhof | 4:c45eaa904b09 | 113 | float k = 0.2; //stiffness |
jordymorsinkhof | 3:98ea6afa0cf2 | 114 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 115 | //Current position = VARIABLE EXPRESSED IN BASE FRAME |
jordymorsinkhof | 4:c45eaa904b09 | 116 | float rx1 = -14.2; //x coordinate of joint 1 |
jordymorsinkhof | 4:c45eaa904b09 | 117 | float ry1 = 11.9; //y coordinate of joint 1 |
jordymorsinkhof | 4:c45eaa904b09 | 118 | float rx2 = cos(q1)*L1-14.2; //x coordinate of joint 2 |
jordymorsinkhof | 4:c45eaa904b09 | 119 | float ry2 = sin(q1)*L1+11.9; //y coordinate of joint 2 |
jordymorsinkhof | 3:98ea6afa0cf2 | 120 | float rxe = rx2 + cos(q1+q2)*L2; //x coordinate of end-effector |
jordymorsinkhof | 3:98ea6afa0cf2 | 121 | float rye = ry2 + sin(q1+q2)*L2; //y coordinate of end-effector |
jordymorsinkhof | 4:c45eaa904b09 | 122 | //pc.printf("Current position: rx1 = %f, ry1 = %f, rx2 = %f, ry2 = %f, rxe = %f, rye = %f\n\r", rx1, ry1, rx2, ry2, rxe, rye); |
jordymorsinkhof | 4:c45eaa904b09 | 123 | //pc.printf("Posx , Posy: %f %f \r\n",rxe,rye); |
jordymorsinkhof | 3:98ea6afa0cf2 | 124 | //Define transposed Jacobian J_T [3x2] |
jordymorsinkhof | 3:98ea6afa0cf2 | 125 | float J_T1 = 1; |
jordymorsinkhof | 4:c45eaa904b09 | 126 | float J_T2 = ry1; |
jordymorsinkhof | 4:c45eaa904b09 | 127 | float J_T3 = rx1; |
jordymorsinkhof | 3:98ea6afa0cf2 | 128 | float J_T4 = 1; |
jordymorsinkhof | 4:c45eaa904b09 | 129 | float J_T5 = ry2; |
jordymorsinkhof | 4:c45eaa904b09 | 130 | float J_T6 = -rx2; |
jordymorsinkhof | 3:98ea6afa0cf2 | 131 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 132 | //Define spring force |
jordymorsinkhof | 3:98ea6afa0cf2 | 133 | float Fsprx = k*(Xset - Xcurr); |
jordymorsinkhof | 3:98ea6afa0cf2 | 134 | float Fspry = k*(Yset - Ycurr); |
jordymorsinkhof | 4:c45eaa904b09 | 135 | //pc.printf("The new value of Fsprx is %f, new Fspry is %f\n\r", Fsprx, Fspry); |
jordymorsinkhof | 3:98ea6afa0cf2 | 136 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 137 | //Define wrench Wspr = (tau Fx Fy) = (rxFx - rxFy Fx Fy) |
jordymorsinkhof | 4:c45eaa904b09 | 138 | float Wspr1 = -Ycurr*Fsprx + Xcurr*Fspry; |
jordymorsinkhof | 3:98ea6afa0cf2 | 139 | float Wspr2 = Fsprx; |
jordymorsinkhof | 3:98ea6afa0cf2 | 140 | float Wspr3 = Fspry; |
jordymorsinkhof | 4:c45eaa904b09 | 141 | //pc.printf("Fx , Fy: %f %f \r\n",Fsprx,Fspry); |
jordymorsinkhof | 4:c45eaa904b09 | 142 | |
jordymorsinkhof | 3:98ea6afa0cf2 | 143 | //End-effector wrench to forces and torques on the joints using the Jacobian (transposed) |
jordymorsinkhof | 4:c45eaa904b09 | 144 | //pc.printf("The old value of tau1 was %f, old tau2 was %f\n\r", tau1, tau2); |
jordymorsinkhof | 3:98ea6afa0cf2 | 145 | tau1 = J_T1*Wspr1+J_T2*Wspr2+J_T3*Wspr3; |
jordymorsinkhof | 3:98ea6afa0cf2 | 146 | tau2 = J_T4*Wspr1+J_T5*Wspr2+J_T6*Wspr3; |
jordymorsinkhof | 4:c45eaa904b09 | 147 | //pc.printf("tau Rene: %f %f\r\n",tau1,tau2); |
jordymorsinkhof | 4:c45eaa904b09 | 148 | tau1 = Fspry*(L2*cos(q1+q2)+L1*cos(q1)) - Fsprx*(L2*sin(q1+q2)+L1*sin(q1)); |
jordymorsinkhof | 4:c45eaa904b09 | 149 | tau2 = Fspry*L2*cos(q1+q2) - Fsprx*L2*sin(q1+q2); |
jordymorsinkhof | 4:c45eaa904b09 | 150 | //pc.printf("tau Bram: %f %f\r\n",tau1,tau2); |
jordymorsinkhof | 4:c45eaa904b09 | 151 | //pc.printf("The new value of tau1 is %f, new tau2 is %f\n\r", tau1, tau2); |
jordymorsinkhof | 4:c45eaa904b09 | 152 | //pc.printf("Tau1, Tau2: %f %f \r\n",tau1,tau2); |
jordymorsinkhof | 3:98ea6afa0cf2 | 153 | } |
jordymorsinkhof | 3:98ea6afa0cf2 | 154 | |
jordymorsinkhof | 4:c45eaa904b09 | 155 | void ControlFunction(float AngleRef1,float AngleRef2){ |
jordymorsinkhof | 3:98ea6afa0cf2 | 156 | |
jordymorsinkhof | 4:c45eaa904b09 | 157 | float Angle1 = (EncoderMotor1.getPulses()/4200.0f)*(2.0f*pi)/3.0f; //Motorpos. in radians |
jordymorsinkhof | 4:c45eaa904b09 | 158 | float Angle2 = (EncoderMotor2.getPulses()/4200.0f)*(2.0f*pi)/1.8f; //Motorpos. in radians |
jordymorsinkhof | 2:684d5cf2f683 | 159 | |
jordymorsinkhof | 2:684d5cf2f683 | 160 | // PID input |
jordymorsinkhof | 4:c45eaa904b09 | 161 | controller1.setSetPoint(AngleRef1); |
jordymorsinkhof | 4:c45eaa904b09 | 162 | controller2.setSetPoint(AngleRef2); |
jordymorsinkhof | 4:c45eaa904b09 | 163 | |
jordymorsinkhof | 4:c45eaa904b09 | 164 | //pc.printf("ANGLEREF: %f %f \r\n",AngleRef1,AngleRef2); |
jordymorsinkhof | 4:c45eaa904b09 | 165 | //pc.printf("ANGLE : %f %f \r\n\n",Angle1,Angle2); |
jordymorsinkhof | 4:c45eaa904b09 | 166 | |
jordymorsinkhof | 4:c45eaa904b09 | 167 | controller1.setProcessValue(Angle1); |
jordymorsinkhof | 4:c45eaa904b09 | 168 | controller2.setProcessValue(Angle2); |
jordymorsinkhof | 2:684d5cf2f683 | 169 | |
jordymorsinkhof | 2:684d5cf2f683 | 170 | // PID output |
jordymorsinkhof | 2:684d5cf2f683 | 171 | float OutPut1=controller1.compute(); |
jordymorsinkhof | 2:684d5cf2f683 | 172 | float OutPut2=controller2.compute(); |
jordymorsinkhof | 2:684d5cf2f683 | 173 | |
jordymorsinkhof | 2:684d5cf2f683 | 174 | // Direction handling |
jordymorsinkhof | 2:684d5cf2f683 | 175 | float Direction1=0; |
jordymorsinkhof | 2:684d5cf2f683 | 176 | float Direction2=0; |
jordymorsinkhof | 2:684d5cf2f683 | 177 | |
jordymorsinkhof | 2:684d5cf2f683 | 178 | if(OutPut1<0){ |
jordymorsinkhof | 2:684d5cf2f683 | 179 | Direction1=1; |
jordymorsinkhof | 2:684d5cf2f683 | 180 | } |
jordymorsinkhof | 2:684d5cf2f683 | 181 | if(OutPut2<0){ |
jordymorsinkhof | 2:684d5cf2f683 | 182 | Direction2=1; |
jordymorsinkhof | 2:684d5cf2f683 | 183 | } |
jordymorsinkhof | 2:684d5cf2f683 | 184 | |
jordymorsinkhof | 2:684d5cf2f683 | 185 | OutPut1=fabs(OutPut1); |
jordymorsinkhof | 2:684d5cf2f683 | 186 | OutPut2=fabs(OutPut2); |
jordymorsinkhof | 2:684d5cf2f683 | 187 | |
jordymorsinkhof | 2:684d5cf2f683 | 188 | // Output schrijven |
jordymorsinkhof | 2:684d5cf2f683 | 189 | MotorThrottle1.write(OutPut1); |
jordymorsinkhof | 2:684d5cf2f683 | 190 | MotorThrottle2.write(OutPut2); |
jordymorsinkhof | 2:684d5cf2f683 | 191 | MotorDirection1=Direction1; |
jordymorsinkhof | 2:684d5cf2f683 | 192 | MotorDirection2=Direction2; |
BramS23 | 0:6c0f87177797 | 193 | } |
jordymorsinkhof | 4:c45eaa904b09 | 194 | float omg1; //previous values are irrelevant |
jordymorsinkhof | 4:c45eaa904b09 | 195 | float omg2; //previous values are irrelevant |
jordymorsinkhof | 4:c45eaa904b09 | 196 | //Overall function which only needs inputs |
jordymorsinkhof | 4:c45eaa904b09 | 197 | void LoopFunction() |
jordymorsinkhof | 4:c45eaa904b09 | 198 | { |
jordymorsinkhof | 4:c45eaa904b09 | 199 | float q1 = EncoderMotor1.getPulses()/4200.0f*2.0f*pi/3.0f; //Motorpos. in radians |
jordymorsinkhof | 4:c45eaa904b09 | 200 | float q2 = EncoderMotor2.getPulses()/4200.0f*2.0f*pi/1.8f; //Motorpos. in radians |
jordymorsinkhof | 4:c45eaa904b09 | 201 | |
jordymorsinkhof | 4:c45eaa904b09 | 202 | //start values |
jordymorsinkhof | 4:c45eaa904b09 | 203 | float tau1; //previous values are irrelevant |
jordymorsinkhof | 4:c45eaa904b09 | 204 | float tau2; //previous values are irrelevant |
jordymorsinkhof | 4:c45eaa904b09 | 205 | |
jordymorsinkhof | 4:c45eaa904b09 | 206 | float Xcurr; //new value is calculated, old replaced |
jordymorsinkhof | 4:c45eaa904b09 | 207 | float Ycurr; //new value is calculated, old replaced |
jordymorsinkhof | 4:c45eaa904b09 | 208 | |
jordymorsinkhof | 4:c45eaa904b09 | 209 | float b = 200; //damping |
jordymorsinkhof | 4:c45eaa904b09 | 210 | |
jordymorsinkhof | 4:c45eaa904b09 | 211 | // Call function to calculate Xcurr and Ycurr from q1 and q2 |
jordymorsinkhof | 4:c45eaa904b09 | 212 | Brock(q1, q2, Xcurr, Ycurr); |
jordymorsinkhof | 4:c45eaa904b09 | 213 | |
jordymorsinkhof | 4:c45eaa904b09 | 214 | // Call function to calculate tau1 and tau2 from X,Ycurr and X,Yset |
jordymorsinkhof | 4:c45eaa904b09 | 215 | ErrorToTau(q1, q2, Xcurr, Ycurr, GXset, GYset, tau1, tau2); |
jordymorsinkhof | 4:c45eaa904b09 | 216 | |
jordymorsinkhof | 4:c45eaa904b09 | 217 | //torque to joint velocity |
jordymorsinkhof | 4:c45eaa904b09 | 218 | //pc.printf("The old value of omg1 was %f, old omg2 was %f\n\r", omg1, omg2); |
jordymorsinkhof | 4:c45eaa904b09 | 219 | omg1 = tau1/b; |
jordymorsinkhof | 4:c45eaa904b09 | 220 | omg2 = tau2/b; |
jordymorsinkhof | 4:c45eaa904b09 | 221 | //pc.printf("The new value of omg1 is %f, new omg2 is %f\n\r", omg1, omg2); |
jordymorsinkhof | 4:c45eaa904b09 | 222 | |
jordymorsinkhof | 4:c45eaa904b09 | 223 | //joint velocity to angles q1 and q2, where you define new q1 and q2 based on previous q1 and q2 |
jordymorsinkhof | 4:c45eaa904b09 | 224 | q1set = q1set + omg1*Interval; |
jordymorsinkhof | 4:c45eaa904b09 | 225 | q2set = q2set + omg2*Interval; |
jordymorsinkhof | 4:c45eaa904b09 | 226 | |
jordymorsinkhof | 4:c45eaa904b09 | 227 | //pc.printf("q1set , q2set: %f %f \r\n",q1set,q2set); |
jordymorsinkhof | 4:c45eaa904b09 | 228 | //pc.printf("q1 , q2 : %f %f \r\n",q1,q2); |
jordymorsinkhof | 4:c45eaa904b09 | 229 | |
jordymorsinkhof | 4:c45eaa904b09 | 230 | ControlFunction(q1set,q2set); |
jordymorsinkhof | 4:c45eaa904b09 | 231 | } |
jordymorsinkhof | 4:c45eaa904b09 | 232 | |
jordymorsinkhof | 4:c45eaa904b09 | 233 | |
jordymorsinkhof | 4:c45eaa904b09 | 234 | void HomingLoop(){ |
jordymorsinkhof | 4:c45eaa904b09 | 235 | |
jordymorsinkhof | 4:c45eaa904b09 | 236 | EndSwitch1.mode(PullUp); |
jordymorsinkhof | 4:c45eaa904b09 | 237 | EndSwitch2.mode(PullUp); |
jordymorsinkhof | 4:c45eaa904b09 | 238 | |
jordymorsinkhof | 4:c45eaa904b09 | 239 | MotorThrottle1=0.2f; |
jordymorsinkhof | 4:c45eaa904b09 | 240 | MotorThrottle2=0.1f; |
jordymorsinkhof | 4:c45eaa904b09 | 241 | |
jordymorsinkhof | 4:c45eaa904b09 | 242 | MotorDirection1=1; |
jordymorsinkhof | 4:c45eaa904b09 | 243 | MotorDirection2=1; |
jordymorsinkhof | 4:c45eaa904b09 | 244 | |
jordymorsinkhof | 4:c45eaa904b09 | 245 | bool dummy1=0; |
jordymorsinkhof | 4:c45eaa904b09 | 246 | bool dummy2=0; |
jordymorsinkhof | 4:c45eaa904b09 | 247 | |
jordymorsinkhof | 4:c45eaa904b09 | 248 | while(EndSwitch1.read()|EndSwitch2.read()){ |
jordymorsinkhof | 4:c45eaa904b09 | 249 | if((EndSwitch1.read()==0)&&(dummy1==0)){ |
jordymorsinkhof | 4:c45eaa904b09 | 250 | MotorThrottle1=0.0f; |
jordymorsinkhof | 4:c45eaa904b09 | 251 | dummy1=1; |
jordymorsinkhof | 4:c45eaa904b09 | 252 | } |
jordymorsinkhof | 4:c45eaa904b09 | 253 | if((EndSwitch2.read()==0)&&(dummy2==0)){ |
jordymorsinkhof | 4:c45eaa904b09 | 254 | MotorThrottle2=0.0f; |
jordymorsinkhof | 4:c45eaa904b09 | 255 | dummy2=1; |
jordymorsinkhof | 4:c45eaa904b09 | 256 | } |
jordymorsinkhof | 4:c45eaa904b09 | 257 | } |
jordymorsinkhof | 4:c45eaa904b09 | 258 | MotorThrottle1=0.0f; |
jordymorsinkhof | 4:c45eaa904b09 | 259 | MotorThrottle2=0.0f; |
jordymorsinkhof | 4:c45eaa904b09 | 260 | EncoderMotor1.reset(0.29/2/pi*4200*3.0f); |
jordymorsinkhof | 4:c45eaa904b09 | 261 | EncoderMotor2.reset((3.3715-2*pi)/2/pi*4200*1.8f); |
jordymorsinkhof | 4:c45eaa904b09 | 262 | } |
jordymorsinkhof | 4:c45eaa904b09 | 263 | |
jordymorsinkhof | 4:c45eaa904b09 | 264 | |
BramS23 | 0:6c0f87177797 | 265 | |
BramS23 | 0:6c0f87177797 | 266 | |
jordymorsinkhof | 2:684d5cf2f683 | 267 | void PickUp(){ |
jordymorsinkhof | 2:684d5cf2f683 | 268 | |
jordymorsinkhof | 2:684d5cf2f683 | 269 | } |
jordymorsinkhof | 2:684d5cf2f683 | 270 | |
jordymorsinkhof | 2:684d5cf2f683 | 271 | void LayDown(){ |
jordymorsinkhof | 2:684d5cf2f683 | 272 | |
jordymorsinkhof | 2:684d5cf2f683 | 273 | |
jordymorsinkhof | 4:c45eaa904b09 | 274 | } |
jordymorsinkhof | 2:684d5cf2f683 | 275 | |
jordymorsinkhof | 2:684d5cf2f683 | 276 | |
jordymorsinkhof | 2:684d5cf2f683 | 277 | void RemoteController(){ |
jordymorsinkhof | 2:684d5cf2f683 | 278 | |
jordymorsinkhof | 2:684d5cf2f683 | 279 | int bitcount; |
jordymorsinkhof | 2:684d5cf2f683 | 280 | |
jordymorsinkhof | 2:684d5cf2f683 | 281 | |
jordymorsinkhof | 2:684d5cf2f683 | 282 | bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8); |
jordymorsinkhof | 2:684d5cf2f683 | 283 | int state = buf[2]; |
jordymorsinkhof | 2:684d5cf2f683 | 284 | switch(state) |
jordymorsinkhof | 2:684d5cf2f683 | 285 | { |
jordymorsinkhof | 2:684d5cf2f683 | 286 | case 22: //1 |
jordymorsinkhof | 3:98ea6afa0cf2 | 287 | pc.printf("1\n\r"); |
jordymorsinkhof | 2:684d5cf2f683 | 288 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 289 | case 25: //1 |
jordymorsinkhof | 3:98ea6afa0cf2 | 290 | pc.printf("2\n\r"); |
jordymorsinkhof | 2:684d5cf2f683 | 291 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 292 | case 13: //1 |
jordymorsinkhof | 3:98ea6afa0cf2 | 293 | pc.printf("3\n\r"); |
jordymorsinkhof | 2:684d5cf2f683 | 294 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 295 | case 12: //1 |
jordymorsinkhof | 3:98ea6afa0cf2 | 296 | pc.printf("4\n\r"); |
jordymorsinkhof | 2:684d5cf2f683 | 297 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 298 | case 24: //1 |
jordymorsinkhof | 3:98ea6afa0cf2 | 299 | pc.printf("5\n\r"); |
jordymorsinkhof | 2:684d5cf2f683 | 300 | ControlTicker.detach(); |
jordymorsinkhof | 2:684d5cf2f683 | 301 | MotorThrottle1=0.0f; |
jordymorsinkhof | 2:684d5cf2f683 | 302 | MotorThrottle2=0.0f; |
jordymorsinkhof | 2:684d5cf2f683 | 303 | PickUp(); |
jordymorsinkhof | 4:c45eaa904b09 | 304 | //ControlTicker.attach(&ControlLoop, Interval); |
jordymorsinkhof | 2:684d5cf2f683 | 305 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 306 | case 94: //1 |
jordymorsinkhof | 3:98ea6afa0cf2 | 307 | pc.printf("6\n\r"); |
jordymorsinkhof | 2:684d5cf2f683 | 308 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 309 | case 8: //1 |
jordymorsinkhof | 3:98ea6afa0cf2 | 310 | pc.printf("7\n\r"); |
jordymorsinkhof | 2:684d5cf2f683 | 311 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 312 | case 28: //1 |
jordymorsinkhof | 3:98ea6afa0cf2 | 313 | pc.printf("8\n\r"); |
jordymorsinkhof | 2:684d5cf2f683 | 314 | ControlTicker.detach(); |
jordymorsinkhof | 2:684d5cf2f683 | 315 | MotorThrottle1=0.0f; |
jordymorsinkhof | 2:684d5cf2f683 | 316 | MotorThrottle2=0.0f; |
jordymorsinkhof | 2:684d5cf2f683 | 317 | LayDown(); |
jordymorsinkhof | 4:c45eaa904b09 | 318 | //ControlTicker.attach(&ControlLoop, Interval); |
jordymorsinkhof | 2:684d5cf2f683 | 319 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 320 | case 90: //1 |
jordymorsinkhof | 3:98ea6afa0cf2 | 321 | pc.printf("9\n\r"); |
jordymorsinkhof | 2:684d5cf2f683 | 322 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 323 | case 70: //1 |
jordymorsinkhof | 2:684d5cf2f683 | 324 | pc.printf("Boven\n\r"); |
jordymorsinkhof | 4:c45eaa904b09 | 325 | //PosRef2=PosRef2-0.1f; |
jordymorsinkhof | 4:c45eaa904b09 | 326 | /* |
jordymorsinkhof | 4:c45eaa904b09 | 327 | GYset = GYset + 1; |
jordymorsinkhof | 4:c45eaa904b09 | 328 | */ |
jordymorsinkhof | 4:c45eaa904b09 | 329 | //pc.printf("%f\n\r", PosRef2); |
jordymorsinkhof | 2:684d5cf2f683 | 330 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 331 | case 21: //1 |
jordymorsinkhof | 2:684d5cf2f683 | 332 | pc.printf("Onder\n\r"); |
jordymorsinkhof | 4:c45eaa904b09 | 333 | //PosRef2=PosRef2+0.1f; |
jordymorsinkhof | 4:c45eaa904b09 | 334 | /* |
jordymorsinkhof | 4:c45eaa904b09 | 335 | GYset = GYset - 1; |
jordymorsinkhof | 4:c45eaa904b09 | 336 | */ |
jordymorsinkhof | 4:c45eaa904b09 | 337 | //pc.printf("%f\n\r", PosRef2); |
jordymorsinkhof | 2:684d5cf2f683 | 338 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 339 | case 68: //1 |
jordymorsinkhof | 2:684d5cf2f683 | 340 | pc.printf("Links\n\r"); |
jordymorsinkhof | 4:c45eaa904b09 | 341 | //PosRef1=PosRef1+0.1f; |
jordymorsinkhof | 4:c45eaa904b09 | 342 | /* |
jordymorsinkhof | 4:c45eaa904b09 | 343 | GXset = GXset + 1; |
jordymorsinkhof | 4:c45eaa904b09 | 344 | */ |
jordymorsinkhof | 4:c45eaa904b09 | 345 | //pc.printf("%f\n\r", PosRef1); |
jordymorsinkhof | 2:684d5cf2f683 | 346 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 347 | case 67: //1 |
jordymorsinkhof | 2:684d5cf2f683 | 348 | pc.printf("Rechts\n\r"); |
jordymorsinkhof | 4:c45eaa904b09 | 349 | //PosRef1=PosRef1-0.1f; |
jordymorsinkhof | 4:c45eaa904b09 | 350 | /* |
jordymorsinkhof | 4:c45eaa904b09 | 351 | GXset = GXset - 1; |
jordymorsinkhof | 4:c45eaa904b09 | 352 | */ |
jordymorsinkhof | 4:c45eaa904b09 | 353 | //pc.printf("%f\n\r", PosRef1); |
jordymorsinkhof | 2:684d5cf2f683 | 354 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 355 | case 64: //1 |
jordymorsinkhof | 2:684d5cf2f683 | 356 | pc.printf("OK\n\r"); |
jordymorsinkhof | 4:c45eaa904b09 | 357 | //ControlTicker.detach(); |
jordymorsinkhof | 4:c45eaa904b09 | 358 | //MotorThrottle1=0.0f; |
jordymorsinkhof | 4:c45eaa904b09 | 359 | //MotorThrottle2=0.0f; |
jordymorsinkhof | 4:c45eaa904b09 | 360 | //HomingLoop(); |
jordymorsinkhof | 4:c45eaa904b09 | 361 | //ControlTicker.attach(&ControlLoop, Interval); |
jordymorsinkhof | 2:684d5cf2f683 | 362 | |
jordymorsinkhof | 2:684d5cf2f683 | 363 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 364 | default: |
jordymorsinkhof | 2:684d5cf2f683 | 365 | break; |
jordymorsinkhof | 2:684d5cf2f683 | 366 | } |
jordymorsinkhof | 2:684d5cf2f683 | 367 | } |
jordymorsinkhof | 2:684d5cf2f683 | 368 | |
jordymorsinkhof | 2:684d5cf2f683 | 369 | |
jordymorsinkhof | 2:684d5cf2f683 | 370 | |
jordymorsinkhof | 4:c45eaa904b09 | 371 | // Give Reference Position |
jordymorsinkhof | 2:684d5cf2f683 | 372 | void DeterminePosRef(){ |
jordymorsinkhof | 4:c45eaa904b09 | 373 | GXset=40*PotMeter1.read(); // Reference in Rad |
jordymorsinkhof | 4:c45eaa904b09 | 374 | GYset=40*PotMeter2.read(); // Reference in Rad |
jordymorsinkhof | 4:c45eaa904b09 | 375 | pc.printf("RefX , RefY: %f %f\r\n",GXset,GYset); |
jordymorsinkhof | 4:c45eaa904b09 | 376 | float posx=0; |
jordymorsinkhof | 4:c45eaa904b09 | 377 | float posy=0; |
jordymorsinkhof | 4:c45eaa904b09 | 378 | Brock(EncoderMotor1.getPulses()/4200.0f*2.0f*pi/3.0f,EncoderMotor2.getPulses()/4200.0f*2.0f*pi/1.8f,posx,posy); |
jordymorsinkhof | 4:c45eaa904b09 | 379 | pc.printf("posx posy : %f %f\r\n",posx,posy); |
jordymorsinkhof | 4:c45eaa904b09 | 380 | pc.printf("q1set,q2set: %f %f\r\n",q1set,q2set); |
jordymorsinkhof | 4:c45eaa904b09 | 381 | pc.printf("q1 ,q2 : %f %f\r\n",EncoderMotor1.getPulses()/4200.0f*2.0f*pi/3.0f,EncoderMotor2.getPulses()/4200.0f*2.0f*pi/1.8f); |
jordymorsinkhof | 4:c45eaa904b09 | 382 | pc.printf("w1 ,w2 : %f %f\r\n",omg1,omg2); |
jordymorsinkhof | 4:c45eaa904b09 | 383 | pc.printf("\n"); |
jordymorsinkhof | 4:c45eaa904b09 | 384 | //ControlFunction(GXset,GYset); |
jordymorsinkhof | 2:684d5cf2f683 | 385 | } |
jordymorsinkhof | 2:684d5cf2f683 | 386 | |
BramS23 | 0:6c0f87177797 | 387 | int main() { |
BramS23 | 0:6c0f87177797 | 388 | pc.baud(115200); |
jordymorsinkhof | 4:c45eaa904b09 | 389 | pc.printf("startup\r\n"); |
jordymorsinkhof | 2:684d5cf2f683 | 390 | |
jordymorsinkhof | 4:c45eaa904b09 | 391 | controller1.setInputLimits(-2.0f*pi,2.0f*pi); |
jordymorsinkhof | 4:c45eaa904b09 | 392 | controller2.setInputLimits(-2.0f*pi,2.0f*pi); |
jordymorsinkhof | 2:684d5cf2f683 | 393 | controller1.setOutputLimits(-0.15f,0.15f); |
jordymorsinkhof | 2:684d5cf2f683 | 394 | controller2.setOutputLimits(-0.5f,0.5f); |
jordymorsinkhof | 4:c45eaa904b09 | 395 | |
jordymorsinkhof | 4:c45eaa904b09 | 396 | pc.printf("Homing \r\n"); |
jordymorsinkhof | 2:684d5cf2f683 | 397 | HomingLoop(); |
jordymorsinkhof | 4:c45eaa904b09 | 398 | pc.printf("Starting Tickers \r\n"); |
jordymorsinkhof | 4:c45eaa904b09 | 399 | ControlTicker.attach(&LoopFunction,Interval); |
jordymorsinkhof | 4:c45eaa904b09 | 400 | GetRefTicker.attach(&DeterminePosRef,0.5f); |
jordymorsinkhof | 2:684d5cf2f683 | 401 | |
jordymorsinkhof | 2:684d5cf2f683 | 402 | |
jordymorsinkhof | 2:684d5cf2f683 | 403 | while(1) |
jordymorsinkhof | 2:684d5cf2f683 | 404 | { |
jordymorsinkhof | 2:684d5cf2f683 | 405 | if (ir_rx.getState() == ReceiverIR::Received) |
jordymorsinkhof | 2:684d5cf2f683 | 406 | { |
jordymorsinkhof | 2:684d5cf2f683 | 407 | pc.printf("received"); |
jordymorsinkhof | 4:c45eaa904b09 | 408 | |
jordymorsinkhof | 4:c45eaa904b09 | 409 | |
jordymorsinkhof | 2:684d5cf2f683 | 410 | RemoteController(); |
jordymorsinkhof | 4:c45eaa904b09 | 411 | wait(0.1); |
BramS23 | 0:6c0f87177797 | 412 | } |
jordymorsinkhof | 2:684d5cf2f683 | 413 | } |
jordymorsinkhof | 2:684d5cf2f683 | 414 | |
jordymorsinkhof | 2:684d5cf2f683 | 415 | |
jordymorsinkhof | 2:684d5cf2f683 | 416 | while(1){} |
jordymorsinkhof | 2:684d5cf2f683 | 417 | |
jordymorsinkhof | 2:684d5cf2f683 | 418 | |
BramS23 | 0:6c0f87177797 | 419 | } |