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@3:98ea6afa0cf2, 2017-10-26 (annotated)

Committer:
jordymorsinkhof
Date:
Thu Oct 26 19:48:03 2017 +0000
Revision:
3:98ea6afa0cf2
Parent:
2:684d5cf2f683
Child:
4:c45eaa904b09
Program met programma van rene ( nog niet werkend) ; q1set is nog niet gelijk gesteld aan posref;

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 2:684d5cf2f683 34 PID controller1(20.0f,0.0f,0.001f, Interval);
jordymorsinkhof 2:684d5cf2f683 35 PID controller2(20.0f,0.0f,0.002f, Interval);
jordymorsinkhof 2:684d5cf2f683 36
jordymorsinkhof 3:98ea6afa0cf2 37 //G: elsewhere given values
jordymorsinkhof 3:98ea6afa0cf2 38 float Gq1 = 0; //from homing
jordymorsinkhof 3:98ea6afa0cf2 39 float Gq2 = 0; //from homing
jordymorsinkhof 3:98ea6afa0cf2 40 float GXset = 47.5; //from EMG in cm
jordymorsinkhof 3:98ea6afa0cf2 41 float GYset = 11; //from EMG in cm
jordymorsinkhof 3:98ea6afa0cf2 42
jordymorsinkhof 3:98ea6afa0cf2 43 //Constant robot parameters
jordymorsinkhof 3:98ea6afa0cf2 44 const float L1 = 27.5; //length arm 1 in cm
jordymorsinkhof 3:98ea6afa0cf2 45 const float L2 = 32; //length arm 2 in cm
jordymorsinkhof 3:98ea6afa0cf2 46
jordymorsinkhof 3:98ea6afa0cf2 47
jordymorsinkhof 2:684d5cf2f683 48 RemoteIR::Format format;
jordymorsinkhof 2:684d5cf2f683 49 uint8_t buf[4];
BramS23 0:6c0f87177797 50
BramS23 0:6c0f87177797 51 Serial pc(PTB17,PTB16);
BramS23 0:6c0f87177797 52
jordymorsinkhof 2:684d5cf2f683 53 float PosRef1=1.0f;
jordymorsinkhof 2:684d5cf2f683 54 float PosRef2=1.0f;
jordymorsinkhof 2:684d5cf2f683 55
jordymorsinkhof 3:98ea6afa0cf2 56 //Function INPUT: q1, q2. OUTPUT: Xcurr, Ycurr
jordymorsinkhof 3:98ea6afa0cf2 57 void Brock(float q1, float q2, float& Xcurr, float& Ycurr)
jordymorsinkhof 3:98ea6afa0cf2 58 {
jordymorsinkhof 3:98ea6afa0cf2 59 //Position of end-effector in base frame when q1 = q2 = 0 to use in He0(0)
jordymorsinkhof 3:98ea6afa0cf2 60
jordymorsinkhof 3:98ea6afa0cf2 61 float He1[9];
jordymorsinkhof 3:98ea6afa0cf2 62
jordymorsinkhof 3:98ea6afa0cf2 63 He1[0] = cos(q1 + q2);
jordymorsinkhof 3:98ea6afa0cf2 64 He1[1] = -sin(q1 +q2);
jordymorsinkhof 3:98ea6afa0cf2 65 He1[2] = L2*cos(q1 + q2) + L1*cos(q1);
jordymorsinkhof 3:98ea6afa0cf2 66 He1[3] = sin(q1 + q2);
jordymorsinkhof 3:98ea6afa0cf2 67 He1[4] = cos(q1 + q2);
jordymorsinkhof 3:98ea6afa0cf2 68 He1[5] = L2*sin(q1 + q2) + L1*sin(q1);
jordymorsinkhof 3:98ea6afa0cf2 69 He1[6] = 0;
jordymorsinkhof 3:98ea6afa0cf2 70 He1[7] = 0;
jordymorsinkhof 3:98ea6afa0cf2 71 He1[8] = 1;
jordymorsinkhof 3:98ea6afa0cf2 72
jordymorsinkhof 3:98ea6afa0cf2 73 // print He1 to check the matrix
jordymorsinkhof 3:98ea6afa0cf2 74 pc.printf("He1 = [\n\r");
jordymorsinkhof 3:98ea6afa0cf2 75
jordymorsinkhof 3:98ea6afa0cf2 76 for (int i=0; i<=8; i++){
jordymorsinkhof 2:684d5cf2f683 77
jordymorsinkhof 3:98ea6afa0cf2 78 pc.printf("%.2f ",He1[i]);
jordymorsinkhof 3:98ea6afa0cf2 79 if (i==2){
jordymorsinkhof 3:98ea6afa0cf2 80 pc.printf("\n\r");}
jordymorsinkhof 3:98ea6afa0cf2 81 else if (i==5){
jordymorsinkhof 3:98ea6afa0cf2 82 pc.printf("\n\r");}
jordymorsinkhof 3:98ea6afa0cf2 83 }
jordymorsinkhof 3:98ea6afa0cf2 84 pc.printf("]\n\r" );
jordymorsinkhof 3:98ea6afa0cf2 85
jordymorsinkhof 3:98ea6afa0cf2 86 //Translation from base frame to board frame SO FROM NOW ON ALL EXPRESSED IN BOARD FRAME
jordymorsinkhof 3:98ea6afa0cf2 87 He1[2] = He1[2] - 12; //12 = x distance from base frame to board frame
jordymorsinkhof 3:98ea6afa0cf2 88 He1[5] = He1[5] + 11; //11 = y distance from base frame to board frame
jordymorsinkhof 3:98ea6afa0cf2 89
jordymorsinkhof 3:98ea6afa0cf2 90 pc.printf("The old value of Xcurr was %f, old Ycurr was %f\n\r", Xcurr, Ycurr);
jordymorsinkhof 3:98ea6afa0cf2 91 Xcurr = He1[2];
jordymorsinkhof 3:98ea6afa0cf2 92 Ycurr = He1[5];
jordymorsinkhof 3:98ea6afa0cf2 93 pc.printf("The new value of Xcurr is %f, new Ycurr is %f\n\r", Xcurr, Ycurr);
jordymorsinkhof 3:98ea6afa0cf2 94
jordymorsinkhof 3:98ea6afa0cf2 95 // print He0 to check the matrix
jordymorsinkhof 3:98ea6afa0cf2 96 pc.printf("\n\r He0 = [\n\r");
jordymorsinkhof 3:98ea6afa0cf2 97 for (int i=0; i<=8; i++){
jordymorsinkhof 3:98ea6afa0cf2 98
jordymorsinkhof 3:98ea6afa0cf2 99 pc.printf("%.2f ",He1[i]);
jordymorsinkhof 3:98ea6afa0cf2 100 if (i==2){
jordymorsinkhof 3:98ea6afa0cf2 101 pc.printf("\n\r");}
jordymorsinkhof 3:98ea6afa0cf2 102 else if (i==5){
jordymorsinkhof 3:98ea6afa0cf2 103 pc.printf("\n\r");}
jordymorsinkhof 3:98ea6afa0cf2 104 }
jordymorsinkhof 3:98ea6afa0cf2 105 pc.printf("]\n\r" );
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 3:98ea6afa0cf2 113 float k = 0.5; //stiffness
jordymorsinkhof 3:98ea6afa0cf2 114
jordymorsinkhof 3:98ea6afa0cf2 115 //Current position = VARIABLE EXPRESSED IN BASE FRAME
jordymorsinkhof 3:98ea6afa0cf2 116 float rx1 = 0; //x coordinate of joint 1
jordymorsinkhof 3:98ea6afa0cf2 117 float ry1 = 0; //y coordinate of joint 1
jordymorsinkhof 3:98ea6afa0cf2 118 float rx2 = cos(q1)*L1; //x coordinate of joint 2
jordymorsinkhof 3:98ea6afa0cf2 119 float ry2 = sin(q1)*L1; //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 3:98ea6afa0cf2 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 3:98ea6afa0cf2 123
jordymorsinkhof 3:98ea6afa0cf2 124 //Define transposed Jacobian J_T [3x2]
jordymorsinkhof 3:98ea6afa0cf2 125 float J_T1 = 1;
jordymorsinkhof 3:98ea6afa0cf2 126 float J_T2 = ry2;
jordymorsinkhof 3:98ea6afa0cf2 127 float J_T3 = -rx2;
jordymorsinkhof 3:98ea6afa0cf2 128 float J_T4 = 1;
jordymorsinkhof 3:98ea6afa0cf2 129 float J_T5 = rye;
jordymorsinkhof 3:98ea6afa0cf2 130 float J_T6 = -rxe;
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 3:98ea6afa0cf2 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 3:98ea6afa0cf2 138 float Wspr1 = Ycurr*Fsprx - Xcurr*Fspry;
jordymorsinkhof 3:98ea6afa0cf2 139 float Wspr2 = Fsprx;
jordymorsinkhof 3:98ea6afa0cf2 140 float Wspr3 = Fspry;
jordymorsinkhof 3:98ea6afa0cf2 141
jordymorsinkhof 3:98ea6afa0cf2 142 //End-effector wrench to forces and torques on the joints using the Jacobian (transposed)
jordymorsinkhof 3:98ea6afa0cf2 143 pc.printf("The old value of tau1 was %f, old tau2 was %f\n\r", tau1, tau2);
jordymorsinkhof 3:98ea6afa0cf2 144 tau1 = J_T1*Wspr1+J_T2*Wspr2+J_T3*Wspr3;
jordymorsinkhof 3:98ea6afa0cf2 145 tau2 = J_T4*Wspr1+J_T5*Wspr2+J_T6*Wspr3;
jordymorsinkhof 3:98ea6afa0cf2 146 pc.printf("The new value of tau1 is %f, new tau2 is %f\n\r", tau1, tau2);
jordymorsinkhof 3:98ea6afa0cf2 147 }
jordymorsinkhof 3:98ea6afa0cf2 148
jordymorsinkhof 3:98ea6afa0cf2 149
jordymorsinkhof 3:98ea6afa0cf2 150 //Overall function which only needs inputs
jordymorsinkhof 3:98ea6afa0cf2 151 void SuperFunction(float q1, float q2, float Xset, float Yset, float& q1set, float& q2set)
jordymorsinkhof 3:98ea6afa0cf2 152 {
jordymorsinkhof 3:98ea6afa0cf2 153 //start values
jordymorsinkhof 3:98ea6afa0cf2 154 float tau1; //previous values are irrelevant
jordymorsinkhof 3:98ea6afa0cf2 155 float tau2; //previous values are irrelevant
jordymorsinkhof 3:98ea6afa0cf2 156 float omg1; //previous values are irrelevant
jordymorsinkhof 3:98ea6afa0cf2 157 float omg2; //previous values are irrelevant
jordymorsinkhof 3:98ea6afa0cf2 158 float Xcurr; //new value is calculated, old replaced
jordymorsinkhof 3:98ea6afa0cf2 159 float Ycurr; //new value is calculated, old replaced
jordymorsinkhof 3:98ea6afa0cf2 160
jordymorsinkhof 3:98ea6afa0cf2 161 float T = 0.01; //Loop period
jordymorsinkhof 3:98ea6afa0cf2 162 float b = 200; //damping
jordymorsinkhof 3:98ea6afa0cf2 163
jordymorsinkhof 3:98ea6afa0cf2 164 // Call function to calculate Xcurr and Ycurr from q1 and q2
jordymorsinkhof 3:98ea6afa0cf2 165 Brock(q1, q2, Xcurr, Ycurr);
jordymorsinkhof 3:98ea6afa0cf2 166
jordymorsinkhof 3:98ea6afa0cf2 167 // Call function to calculate tau1 and tau2 from X,Ycurr and X,Yset
jordymorsinkhof 3:98ea6afa0cf2 168 ErrorToTau(q1, q2, Xcurr, Ycurr, Xset, Yset, tau1, tau2);
jordymorsinkhof 3:98ea6afa0cf2 169
jordymorsinkhof 3:98ea6afa0cf2 170 //torque to joint velocity
jordymorsinkhof 3:98ea6afa0cf2 171 pc.printf("The old value of omg1 was %f, old omg2 was %f\n\r", omg1, omg2);
jordymorsinkhof 3:98ea6afa0cf2 172 omg1 = tau1/b;
jordymorsinkhof 3:98ea6afa0cf2 173 omg2 = tau2/b;
jordymorsinkhof 3:98ea6afa0cf2 174 pc.printf("The new value of omg1 is %f, new omg2 is %f\n\r", omg1, omg2);
jordymorsinkhof 3:98ea6afa0cf2 175
jordymorsinkhof 3:98ea6afa0cf2 176 //joint velocity to angles q1 and q2, where you define new q1 and q2 based on previous q1 and q2
jordymorsinkhof 3:98ea6afa0cf2 177 q1set = q1set + omg1*T;
jordymorsinkhof 3:98ea6afa0cf2 178 q2set = q2set + omg2*T;
jordymorsinkhof 3:98ea6afa0cf2 179 }
jordymorsinkhof 2:684d5cf2f683 180
jordymorsinkhof 2:684d5cf2f683 181
jordymorsinkhof 2:684d5cf2f683 182 void HomingLoop(){
jordymorsinkhof 2:684d5cf2f683 183
jordymorsinkhof 2:684d5cf2f683 184 EndSwitch1.mode(PullUp);
jordymorsinkhof 2:684d5cf2f683 185 EndSwitch2.mode(PullUp);
jordymorsinkhof 2:684d5cf2f683 186
jordymorsinkhof 2:684d5cf2f683 187 MotorThrottle1=0.1f;
jordymorsinkhof 2:684d5cf2f683 188 MotorThrottle2=0.2f;
jordymorsinkhof 2:684d5cf2f683 189
jordymorsinkhof 2:684d5cf2f683 190 MotorDirection1=1;
jordymorsinkhof 2:684d5cf2f683 191 MotorDirection2=1;
jordymorsinkhof 2:684d5cf2f683 192
jordymorsinkhof 2:684d5cf2f683 193 bool dummy1=0;
jordymorsinkhof 2:684d5cf2f683 194 bool dummy2=0;
jordymorsinkhof 2:684d5cf2f683 195
jordymorsinkhof 2:684d5cf2f683 196 while(EndSwitch1.read()|EndSwitch2.read()){
jordymorsinkhof 2:684d5cf2f683 197 if((EndSwitch1.read()==0)&&(dummy1==0)){
jordymorsinkhof 2:684d5cf2f683 198 MotorThrottle1=0.0f;
jordymorsinkhof 2:684d5cf2f683 199 dummy1=1;
jordymorsinkhof 2:684d5cf2f683 200 }
jordymorsinkhof 2:684d5cf2f683 201 if((EndSwitch2.read()==0)&&(dummy2==0)){
jordymorsinkhof 2:684d5cf2f683 202 MotorThrottle2=0.0f;
jordymorsinkhof 2:684d5cf2f683 203 dummy2=1;
jordymorsinkhof 2:684d5cf2f683 204 }
jordymorsinkhof 2:684d5cf2f683 205 }
jordymorsinkhof 2:684d5cf2f683 206 MotorThrottle1=0.0f;
jordymorsinkhof 2:684d5cf2f683 207 MotorThrottle2=0.0f;
jordymorsinkhof 2:684d5cf2f683 208 EncoderMotor1.reset();
jordymorsinkhof 2:684d5cf2f683 209 EncoderMotor2.reset();
jordymorsinkhof 2:684d5cf2f683 210 }
jordymorsinkhof 2:684d5cf2f683 211
jordymorsinkhof 2:684d5cf2f683 212 void ControlLoop(){
jordymorsinkhof 2:684d5cf2f683 213
jordymorsinkhof 2:684d5cf2f683 214 float Pos1 = EncoderMotor1.getPulses()/4200.0f*2.0f*pi; //Motorpos. in radians
jordymorsinkhof 2:684d5cf2f683 215 float Pos2 = EncoderMotor2.getPulses()/4200.0f*2.0f*pi; //Motorpos. in radians
jordymorsinkhof 2:684d5cf2f683 216 float error1 = PosRef1 - Pos1;
jordymorsinkhof 2:684d5cf2f683 217 float error2 = PosRef2 - Pos2;
jordymorsinkhof 2:684d5cf2f683 218
jordymorsinkhof 2:684d5cf2f683 219 // PID input
jordymorsinkhof 2:684d5cf2f683 220 controller1.setSetPoint(PosRef1);
jordymorsinkhof 2:684d5cf2f683 221 controller2.setSetPoint(PosRef2);
jordymorsinkhof 2:684d5cf2f683 222
jordymorsinkhof 2:684d5cf2f683 223 controller1.setProcessValue(Pos1);
jordymorsinkhof 2:684d5cf2f683 224 controller2.setProcessValue(Pos2);
jordymorsinkhof 2:684d5cf2f683 225
jordymorsinkhof 2:684d5cf2f683 226 // PID output
jordymorsinkhof 2:684d5cf2f683 227 float OutPut1=controller1.compute();
jordymorsinkhof 2:684d5cf2f683 228 float OutPut2=controller2.compute();
jordymorsinkhof 2:684d5cf2f683 229
jordymorsinkhof 2:684d5cf2f683 230 // Direction handling
jordymorsinkhof 2:684d5cf2f683 231 float Direction1=0;
jordymorsinkhof 2:684d5cf2f683 232 float Direction2=0;
jordymorsinkhof 2:684d5cf2f683 233
jordymorsinkhof 2:684d5cf2f683 234 if(OutPut1<0){
jordymorsinkhof 2:684d5cf2f683 235 Direction1=1;
jordymorsinkhof 2:684d5cf2f683 236 }
jordymorsinkhof 2:684d5cf2f683 237 if(OutPut2<0){
jordymorsinkhof 2:684d5cf2f683 238 Direction2=1;
jordymorsinkhof 2:684d5cf2f683 239 }
jordymorsinkhof 2:684d5cf2f683 240
jordymorsinkhof 2:684d5cf2f683 241 OutPut1=fabs(OutPut1);
jordymorsinkhof 2:684d5cf2f683 242 OutPut2=fabs(OutPut2);
jordymorsinkhof 2:684d5cf2f683 243
jordymorsinkhof 2:684d5cf2f683 244 // Output schrijven
jordymorsinkhof 2:684d5cf2f683 245 MotorThrottle1.write(OutPut1);
jordymorsinkhof 2:684d5cf2f683 246 MotorThrottle2.write(OutPut2);
jordymorsinkhof 2:684d5cf2f683 247 MotorDirection1=Direction1;
jordymorsinkhof 2:684d5cf2f683 248 MotorDirection2=Direction2;
BramS23 0:6c0f87177797 249 }
BramS23 0:6c0f87177797 250
BramS23 0:6c0f87177797 251
jordymorsinkhof 2:684d5cf2f683 252 void PickUp(){
jordymorsinkhof 2:684d5cf2f683 253
jordymorsinkhof 2:684d5cf2f683 254 }
jordymorsinkhof 2:684d5cf2f683 255
jordymorsinkhof 2:684d5cf2f683 256 void LayDown(){
jordymorsinkhof 2:684d5cf2f683 257
jordymorsinkhof 2:684d5cf2f683 258
jordymorsinkhof 2:684d5cf2f683 259 }
jordymorsinkhof 2:684d5cf2f683 260
jordymorsinkhof 2:684d5cf2f683 261
jordymorsinkhof 2:684d5cf2f683 262 void RemoteController(){
jordymorsinkhof 2:684d5cf2f683 263
jordymorsinkhof 2:684d5cf2f683 264 int bitcount;
jordymorsinkhof 2:684d5cf2f683 265
jordymorsinkhof 2:684d5cf2f683 266
jordymorsinkhof 2:684d5cf2f683 267 bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
jordymorsinkhof 2:684d5cf2f683 268 int state = buf[2];
jordymorsinkhof 2:684d5cf2f683 269 switch(state)
jordymorsinkhof 2:684d5cf2f683 270 {
jordymorsinkhof 2:684d5cf2f683 271 case 22: //1
jordymorsinkhof 3:98ea6afa0cf2 272 pc.printf("1\n\r");
jordymorsinkhof 2:684d5cf2f683 273 break;
jordymorsinkhof 2:684d5cf2f683 274 case 25: //1
jordymorsinkhof 3:98ea6afa0cf2 275 pc.printf("2\n\r");
jordymorsinkhof 2:684d5cf2f683 276 break;
jordymorsinkhof 2:684d5cf2f683 277 case 13: //1
jordymorsinkhof 3:98ea6afa0cf2 278 pc.printf("3\n\r");
jordymorsinkhof 2:684d5cf2f683 279 break;
jordymorsinkhof 2:684d5cf2f683 280 case 12: //1
jordymorsinkhof 3:98ea6afa0cf2 281 pc.printf("4\n\r");
jordymorsinkhof 2:684d5cf2f683 282 break;
jordymorsinkhof 2:684d5cf2f683 283 case 24: //1
jordymorsinkhof 3:98ea6afa0cf2 284 pc.printf("5\n\r");
jordymorsinkhof 2:684d5cf2f683 285 ControlTicker.detach();
jordymorsinkhof 2:684d5cf2f683 286 MotorThrottle1=0.0f;
jordymorsinkhof 2:684d5cf2f683 287 MotorThrottle2=0.0f;
jordymorsinkhof 2:684d5cf2f683 288 PickUp();
jordymorsinkhof 2:684d5cf2f683 289 ControlTicker.attach(&ControlLoop, Interval);
jordymorsinkhof 2:684d5cf2f683 290 break;
jordymorsinkhof 2:684d5cf2f683 291 case 94: //1
jordymorsinkhof 3:98ea6afa0cf2 292 pc.printf("6\n\r");
jordymorsinkhof 2:684d5cf2f683 293 break;
jordymorsinkhof 2:684d5cf2f683 294 case 8: //1
jordymorsinkhof 3:98ea6afa0cf2 295 pc.printf("7\n\r");
jordymorsinkhof 2:684d5cf2f683 296 break;
jordymorsinkhof 2:684d5cf2f683 297 case 28: //1
jordymorsinkhof 3:98ea6afa0cf2 298 pc.printf("8\n\r");
jordymorsinkhof 2:684d5cf2f683 299 ControlTicker.detach();
jordymorsinkhof 2:684d5cf2f683 300 MotorThrottle1=0.0f;
jordymorsinkhof 2:684d5cf2f683 301 MotorThrottle2=0.0f;
jordymorsinkhof 2:684d5cf2f683 302 LayDown();
jordymorsinkhof 2:684d5cf2f683 303 ControlTicker.attach(&ControlLoop, Interval);
jordymorsinkhof 2:684d5cf2f683 304 break;
jordymorsinkhof 2:684d5cf2f683 305 case 90: //1
jordymorsinkhof 3:98ea6afa0cf2 306 pc.printf("9\n\r");
jordymorsinkhof 2:684d5cf2f683 307 break;
jordymorsinkhof 2:684d5cf2f683 308 case 70: //1
jordymorsinkhof 2:684d5cf2f683 309 pc.printf("Boven\n\r");
jordymorsinkhof 2:684d5cf2f683 310 PosRef2=PosRef2-0.1f;
jordymorsinkhof 2:684d5cf2f683 311 pc.printf("%f\n\r", PosRef2);
jordymorsinkhof 2:684d5cf2f683 312 break;
jordymorsinkhof 2:684d5cf2f683 313 case 21: //1
jordymorsinkhof 2:684d5cf2f683 314 pc.printf("Onder\n\r");
jordymorsinkhof 2:684d5cf2f683 315 PosRef2=PosRef2+0.1f;
jordymorsinkhof 2:684d5cf2f683 316 pc.printf("%f\n\r", PosRef2);
jordymorsinkhof 2:684d5cf2f683 317 break;
jordymorsinkhof 2:684d5cf2f683 318 case 68: //1
jordymorsinkhof 2:684d5cf2f683 319 pc.printf("Links\n\r");
jordymorsinkhof 2:684d5cf2f683 320 PosRef1=PosRef1+0.1f;
jordymorsinkhof 2:684d5cf2f683 321 pc.printf("%f\n\r", PosRef1);
jordymorsinkhof 2:684d5cf2f683 322 break;
jordymorsinkhof 2:684d5cf2f683 323 case 67: //1
jordymorsinkhof 2:684d5cf2f683 324 pc.printf("Rechts\n\r");
jordymorsinkhof 2:684d5cf2f683 325 PosRef1=PosRef1-0.1f;
jordymorsinkhof 2:684d5cf2f683 326 pc.printf("%f\n\r", PosRef1);
jordymorsinkhof 2:684d5cf2f683 327 break;
jordymorsinkhof 2:684d5cf2f683 328 case 64: //1
jordymorsinkhof 2:684d5cf2f683 329 pc.printf("OK\n\r");
jordymorsinkhof 2:684d5cf2f683 330 ControlTicker.detach();
jordymorsinkhof 2:684d5cf2f683 331 MotorThrottle1=0.0f;
jordymorsinkhof 2:684d5cf2f683 332 MotorThrottle2=0.0f;
jordymorsinkhof 2:684d5cf2f683 333 HomingLoop();
jordymorsinkhof 2:684d5cf2f683 334 ControlTicker.attach(&ControlLoop, Interval);
jordymorsinkhof 2:684d5cf2f683 335
jordymorsinkhof 2:684d5cf2f683 336 break;
jordymorsinkhof 2:684d5cf2f683 337 default:
jordymorsinkhof 2:684d5cf2f683 338 break;
jordymorsinkhof 2:684d5cf2f683 339 }
jordymorsinkhof 2:684d5cf2f683 340 }
jordymorsinkhof 2:684d5cf2f683 341
jordymorsinkhof 2:684d5cf2f683 342
jordymorsinkhof 2:684d5cf2f683 343
jordymorsinkhof 2:684d5cf2f683 344
jordymorsinkhof 2:684d5cf2f683 345 void DeterminePosRef(){
jordymorsinkhof 2:684d5cf2f683 346 PosRef1=2*pi*PotMeter1.read(); // Reference in Rad
jordymorsinkhof 2:684d5cf2f683 347 PosRef2=2*pi*PotMeter2.read(); // Reference in Rad
jordymorsinkhof 2:684d5cf2f683 348 }
jordymorsinkhof 2:684d5cf2f683 349
BramS23 0:6c0f87177797 350 int main() {
BramS23 0:6c0f87177797 351 pc.baud(115200);
BramS23 0:6c0f87177797 352 pc.printf("startup");
jordymorsinkhof 2:684d5cf2f683 353
jordymorsinkhof 2:684d5cf2f683 354 controller1.setInputLimits(0.5f,2.0f*pi);
jordymorsinkhof 2:684d5cf2f683 355 controller2.setInputLimits(0.5f,2.0f*pi);
jordymorsinkhof 2:684d5cf2f683 356 controller1.setOutputLimits(-0.15f,0.15f);
jordymorsinkhof 2:684d5cf2f683 357 controller2.setOutputLimits(-0.5f,0.5f);
jordymorsinkhof 2:684d5cf2f683 358
jordymorsinkhof 3:98ea6afa0cf2 359 float q1set; //becomes new value after calling function again
jordymorsinkhof 3:98ea6afa0cf2 360 float q2set; //becomes new value after calling function again
jordymorsinkhof 3:98ea6afa0cf2 361 SuperFunction(Gq1, Gq2, GXset, GYset, q1set, q2set);
jordymorsinkhof 3:98ea6afa0cf2 362 pc.printf("The new value of q1set is %f, the new q2set is %f\n\r", q1set, q2set);
jordymorsinkhof 3:98ea6afa0cf2 363 SuperFunction(Gq1, Gq2, GXset, GYset, q1set, q2set);
jordymorsinkhof 3:98ea6afa0cf2 364 pc.printf("The new value of q1set is %f, the new q2set is %f\n\r", q1set, q2set);
jordymorsinkhof 3:98ea6afa0cf2 365
jordymorsinkhof 2:684d5cf2f683 366
jordymorsinkhof 2:684d5cf2f683 367 HomingLoop();
jordymorsinkhof 2:684d5cf2f683 368
BramS23 0:6c0f87177797 369
jordymorsinkhof 2:684d5cf2f683 370 ControlTicker.attach(&ControlLoop, Interval);
jordymorsinkhof 2:684d5cf2f683 371 //GetRefTicker.attach(&DeterminePosRef, 0.1f);
jordymorsinkhof 2:684d5cf2f683 372
jordymorsinkhof 2:684d5cf2f683 373
jordymorsinkhof 2:684d5cf2f683 374
jordymorsinkhof 2:684d5cf2f683 375 while(1)
jordymorsinkhof 2:684d5cf2f683 376 {
jordymorsinkhof 2:684d5cf2f683 377 if (ir_rx.getState() == ReceiverIR::Received)
jordymorsinkhof 2:684d5cf2f683 378 {
jordymorsinkhof 2:684d5cf2f683 379
jordymorsinkhof 2:684d5cf2f683 380 pc.printf("received");
jordymorsinkhof 2:684d5cf2f683 381 RemoteController();
jordymorsinkhof 2:684d5cf2f683 382 wait(0.01);
BramS23 0:6c0f87177797 383 }
jordymorsinkhof 2:684d5cf2f683 384 }
jordymorsinkhof 2:684d5cf2f683 385
jordymorsinkhof 2:684d5cf2f683 386
jordymorsinkhof 2:684d5cf2f683 387 while(1){}
jordymorsinkhof 2:684d5cf2f683 388
jordymorsinkhof 2:684d5cf2f683 389
BramS23 0:6c0f87177797 390 }