jordy morsinkhof / Mbed 2 deprecated Biorobotics_Motor_Control

RobotArm_ Biorobotics project.

Dependencies:   PID QEI RemoteIR Servo mbed

Fork of Biorobotics_Motor_Control by Bram S

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?

UserRevisionLine numberNew 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 }