jordy morsinkhof / Mbed 2 deprecated BIOROBOTICS_GROUP24

Code is werkend! Wat nog aangepast moet worden *gotopos (beweegt op dit moment nog heel langzaam)

Dependencies:   BrocketJacobian MotorThrottle Olimex_wrapper RemoteIR Servo mbed

Fork of BIOROBOTICS_GROUP24 by Bram S

main.cpp@2:9352744f2f03, 2017-11-01 (annotated)

Committer:
jordymorsinkhof
Date:
Wed Nov 01 19:27:10 2017 +0000
Revision:
2:9352744f2f03
Parent:
1:d4fa7e5e42cc 
code werkend!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BramS23 0:df93928b266c 1 #include "mbed.h"
BramS23 0:df93928b266c 2 #include "QEI.h"
BramS23 0:df93928b266c 3 #include "PID.h"
BramS23 0:df93928b266c 4 #include "Motor.h"
BramS23 0:df93928b266c 5 #include "BrocketJacobian.h"
BramS23 0:df93928b266c 6 #include "ReceiverIR.h"
BramS23 0:df93928b266c 7 #include "Servo.h"
BramS23 0:df93928b266c 8 #include "emg.h"
jordymorsinkhof 1:d4fa7e5e42cc 9 #include "filter.h"
BramS23 0:df93928b266c 10
BramS23 0:df93928b266c 11 // Declare constants etc
jordymorsinkhof 1:d4fa7e5e42cc 12 float Interval=0.01f;
BramS23 0:df93928b266c 13 float pi=3.14159265359;
BramS23 0:df93928b266c 14
BramS23 0:df93928b266c 15 // Declare Analogin in for Potmeter, Can be used for references.
BramS23 0:df93928b266c 16 AnalogIn PotMeter1(A0);
BramS23 0:df93928b266c 17 AnalogIn PotMeter2(A1);
BramS23 0:df93928b266c 18
BramS23 0:df93928b266c 19 // Declare IR receiver
BramS23 0:df93928b266c 20 ReceiverIR ir_rx(D2);
BramS23 0:df93928b266c 21
jordymorsinkhof 1:d4fa7e5e42cc 22 // Declare Servo
jordymorsinkhof 1:d4fa7e5e42cc 23 //Servo ZServo(A5);
jordymorsinkhof 1:d4fa7e5e42cc 24 PwmOut ZServo(A5);
jordymorsinkhof 1:d4fa7e5e42cc 25 // Declare Magnet
jordymorsinkhof 1:d4fa7e5e42cc 26 DigitalOut Magnet(D0);
jordymorsinkhof 1:d4fa7e5e42cc 27
BramS23 0:df93928b266c 28 // Declare motor objects that will control the motor
BramS23 0:df93928b266c 29 Motor Motor1(D5,D4,D12,D13,D9,Interval);
BramS23 0:df93928b266c 30 Motor Motor2(D6,D7,D10,D11,D8,Interval);
BramS23 0:df93928b266c 31
BramS23 0:df93928b266c 32 // Declare EMG shields and variables
jordymorsinkhof 1:d4fa7e5e42cc 33 emg_shield emg1(A2,500);
jordymorsinkhof 1:d4fa7e5e42cc 34 emg_shield emg2(A3,500);
jordymorsinkhof 1:d4fa7e5e42cc 35 emg_shield emg3(A4,500);
BramS23 0:df93928b266c 36 bool EMG_Direction = 0;
jordymorsinkhof 1:d4fa7e5e42cc 37 InterruptIn DirectionButton(NC); // Switch 2
BramS23 0:df93928b266c 38
BramS23 0:df93928b266c 39 // Declare tickers
BramS23 0:df93928b266c 40 Ticker ControlTicker;
BramS23 0:df93928b266c 41 Ticker GetRefTicker;
jordymorsinkhof 1:d4fa7e5e42cc 42 Ticker GotoPosTicker;
BramS23 0:df93928b266c 43
BramS23 0:df93928b266c 44 // Delare the GYset and GXset, which are the positions derived from integrating
BramS23 0:df93928b266c 45 // after the applying the jacobian inverse
BramS23 0:df93928b266c 46 float GXset = 40.5; //from EMG in cm
BramS23 0:df93928b266c 47 float GYset = 11; //from EMG in cm
jordymorsinkhof 1:d4fa7e5e42cc 48
jordymorsinkhof 1:d4fa7e5e42cc 49 // Motor angles in radialen
jordymorsinkhof 1:d4fa7e5e42cc 50 float q1set = 0.29f+0.1f;
jordymorsinkhof 1:d4fa7e5e42cc 51 float q2set = (3.3715f-2.0f*pi)+0.1f;
BramS23 0:df93928b266c 52
jordymorsinkhof 1:d4fa7e5e42cc 53 float x_dot_remote = 0.0f;
jordymorsinkhof 1:d4fa7e5e42cc 54 float y_dot_remote = 0.0f;
BramS23 0:df93928b266c 55
BramS23 0:df93928b266c 56 // Declare stuff for the IR receiver
BramS23 0:df93928b266c 57 RemoteIR::Format format;
BramS23 0:df93928b266c 58 uint8_t buf[4];
BramS23 0:df93928b266c 59
jordymorsinkhof 1:d4fa7e5e42cc 60
jordymorsinkhof 1:d4fa7e5e42cc 61 bool EMG_CONTROL=0;
jordymorsinkhof 1:d4fa7e5e42cc 62
jordymorsinkhof 1:d4fa7e5e42cc 63
BramS23 0:df93928b266c 64 // Declare serial object for setting the baudrate
BramS23 0:df93928b266c 65 RawSerial pc(USBTX,USBRX);
BramS23 0:df93928b266c 66
BramS23 0:df93928b266c 67 void DirectionButtonPressed(){
BramS23 0:df93928b266c 68 EMG_Direction=!EMG_Direction;
BramS23 0:df93928b266c 69 }
BramS23 0:df93928b266c 70
BramS23 0:df93928b266c 71 // Looping function using the Jacobian transposed
BramS23 0:df93928b266c 72 void LoopFunctionJacTransposed()
BramS23 0:df93928b266c 73 {
BramS23 0:df93928b266c 74 float q1 = Motor1.GetPos();
BramS23 0:df93928b266c 75 float q2 = Motor2.GetPos();
BramS23 0:df93928b266c 76
BramS23 0:df93928b266c 77 //start values
BramS23 0:df93928b266c 78 float tau1 = 0.0f; //previous values are irrelevant
BramS23 0:df93928b266c 79 float tau2 = 0.0f; //previous values are irrelevant
BramS23 0:df93928b266c 80
BramS23 0:df93928b266c 81 float Xcurr = 0.0f; //new value is calculated, old replaced
BramS23 0:df93928b266c 82 float Ycurr = 0.0f; //new value is calculated, old replaced
BramS23 0:df93928b266c 83
BramS23 0:df93928b266c 84 float b = 200.0f; //damping
jordymorsinkhof 1:d4fa7e5e42cc 85 float k = 1.0f; //stiffness of the spring pulling on the end effector
BramS23 0:df93928b266c 86
BramS23 0:df93928b266c 87 // Call function to calculate Xcurr and Ycurr from q1 and q2
BramS23 0:df93928b266c 88 Brocket(q1, q2, Xcurr, Ycurr);
BramS23 0:df93928b266c 89
BramS23 0:df93928b266c 90 // Compute spring forces
BramS23 0:df93928b266c 91 float Fx = k*(GXset-Xcurr);
BramS23 0:df93928b266c 92 float Fy = k*(GYset-Ycurr);
BramS23 0:df93928b266c 93
BramS23 0:df93928b266c 94 // Call function to calculate tau1 and tau2 from X,Ycurr and X,Yset
jordymorsinkhof 1:d4fa7e5e42cc 95 TransposeJacobian(q1, q2, Fx, Fy, tau1, tau2);
BramS23 0:df93928b266c 96
BramS23 0:df93928b266c 97 // torque to joint velocity
BramS23 0:df93928b266c 98 float omg1 = tau1/b;
BramS23 0:df93928b266c 99 float omg2 = tau2/b;
jordymorsinkhof 1:d4fa7e5e42cc 100
jordymorsinkhof 1:d4fa7e5e42cc 101 printf("%f %f\r\n",GXset,GYset);
BramS23 0:df93928b266c 102
BramS23 0:df93928b266c 103 // joint velocity to angles q1 and q2, where you define new q1 and q2 based on previous q1 and q2
BramS23 0:df93928b266c 104 q1set = q1set + omg1*Interval;
BramS23 0:df93928b266c 105 q2set = q2set + omg2*Interval;
jordymorsinkhof 1:d4fa7e5e42cc 106 if (q1set>(0.75f*pi)) {q1set=(0.75f*pi);}
jordymorsinkhof 1:d4fa7e5e42cc 107 if (q1set<0.30f) {q1set = 0.30f;}
jordymorsinkhof 1:d4fa7e5e42cc 108 if (q2set>0.0f) {q2set=(0.0f);}
jordymorsinkhof 1:d4fa7e5e42cc 109 if (q2set<-2.79f) {q2set = -2.79f;}
BramS23 0:df93928b266c 110
BramS23 0:df93928b266c 111 // Call the function that controlls the motors
BramS23 0:df93928b266c 112 Motor1.GotoPos(q1set);
BramS23 0:df93928b266c 113 Motor2.GotoPos(q2set);
jordymorsinkhof 1:d4fa7e5e42cc 114 //printf("%f %f\r\n",q1set,q2set);
BramS23 0:df93928b266c 115 }
BramS23 0:df93928b266c 116
jordymorsinkhof 1:d4fa7e5e42cc 117 void GotoPosition(float x, float y){
jordymorsinkhof 1:d4fa7e5e42cc 118 float error =10.0f;
jordymorsinkhof 1:d4fa7e5e42cc 119 LowPass RefLowPass1;
jordymorsinkhof 1:d4fa7e5e42cc 120 LowPass RefLowPass2;
jordymorsinkhof 1:d4fa7e5e42cc 121 RefLowPass1.a=0.99;
jordymorsinkhof 1:d4fa7e5e42cc 122 RefLowPass2.a=0.99;
jordymorsinkhof 1:d4fa7e5e42cc 123
jordymorsinkhof 1:d4fa7e5e42cc 124 float Xcurr; float Ycurr;
jordymorsinkhof 1:d4fa7e5e42cc 125 Brocket(Motor1.GetPos(), Motor2.GetPos(), Xcurr, Ycurr);
jordymorsinkhof 1:d4fa7e5e42cc 126
jordymorsinkhof 1:d4fa7e5e42cc 127 RefLowPass1.yprev=Xcurr;
jordymorsinkhof 1:d4fa7e5e42cc 128 RefLowPass2.yprev=Ycurr;
jordymorsinkhof 1:d4fa7e5e42cc 129
jordymorsinkhof 1:d4fa7e5e42cc 130 GXset=RefLowPass1.filter(x);
jordymorsinkhof 1:d4fa7e5e42cc 131 GYset=RefLowPass2.filter(y);
jordymorsinkhof 1:d4fa7e5e42cc 132
jordymorsinkhof 1:d4fa7e5e42cc 133 GotoPosTicker.attach(&LoopFunctionJacTransposed,Interval);
jordymorsinkhof 1:d4fa7e5e42cc 134 while (error>5.0f){
jordymorsinkhof 1:d4fa7e5e42cc 135 GXset=RefLowPass1.filter(x);
jordymorsinkhof 1:d4fa7e5e42cc 136 GYset=RefLowPass2.filter(y);
jordymorsinkhof 1:d4fa7e5e42cc 137 Brocket(Motor1.GetPos(), Motor2.GetPos(), Xcurr, Ycurr);
jordymorsinkhof 1:d4fa7e5e42cc 138 error = sqrt(pow(Xcurr-x,2)+pow(Ycurr-y,2));
jordymorsinkhof 1:d4fa7e5e42cc 139 wait(0.1);
jordymorsinkhof 1:d4fa7e5e42cc 140 }
jordymorsinkhof 1:d4fa7e5e42cc 141 GotoPosTicker.detach();
jordymorsinkhof 1:d4fa7e5e42cc 142 }
BramS23 0:df93928b266c 143
BramS23 0:df93928b266c 144 void LoopJacInverse(){
BramS23 0:df93928b266c 145 // Get Motor Positions
BramS23 0:df93928b266c 146 float q1 = Motor1.GetPos();
BramS23 0:df93928b266c 147 float q2 = Motor2.GetPos();
BramS23 0:df93928b266c 148
BramS23 0:df93928b266c 149 // Define velocities for control
BramS23 0:df93928b266c 150 float q1_dot=0.0f;
BramS23 0:df93928b266c 151 float q2_dot=0.0f;
BramS23 0:df93928b266c 152
BramS23 0:df93928b266c 153 float vx=0.0f;
BramS23 0:df93928b266c 154 float vy=0.0f;
BramS23 0:df93928b266c 155
jordymorsinkhof 1:d4fa7e5e42cc 156 if (EMG_CONTROL){
jordymorsinkhof 1:d4fa7e5e42cc 157 // Get the velocities from EMG
jordymorsinkhof 1:d4fa7e5e42cc 158 float ScaleFactor = 200.0f;
jordymorsinkhof 1:d4fa7e5e42cc 159 vx=ScaleFactor*(emg1.GetValue()- 2.0f*emg2.GetValue());
jordymorsinkhof 1:d4fa7e5e42cc 160 vy=0.0f;//sclaeFactor*(emg2.GetValue());
jordymorsinkhof 1:d4fa7e5e42cc 161 }
jordymorsinkhof 1:d4fa7e5e42cc 162
jordymorsinkhof 1:d4fa7e5e42cc 163
jordymorsinkhof 1:d4fa7e5e42cc 164 if ( !(x_dot_remote ==0.0f)){
jordymorsinkhof 1:d4fa7e5e42cc 165 vx=x_dot_remote;
jordymorsinkhof 1:d4fa7e5e42cc 166 }
jordymorsinkhof 1:d4fa7e5e42cc 167 if ( !(y_dot_remote == 0.0f)){
jordymorsinkhof 1:d4fa7e5e42cc 168 vy=y_dot_remote;
jordymorsinkhof 1:d4fa7e5e42cc 169 }
jordymorsinkhof 1:d4fa7e5e42cc 170
BramS23 0:df93928b266c 171 // Apply Jacobian Inverse
BramS23 0:df93928b266c 172 InverseJacobian(q1,q2,vx,vy,q1_dot,q2_dot);
BramS23 0:df93928b266c 173
BramS23 0:df93928b266c 174 // Integrate q1dot and q2dot to obtain positions
BramS23 0:df93928b266c 175 q1set += q1_dot*Interval;
BramS23 0:df93928b266c 176 q2set += q2_dot*Interval;
BramS23 0:df93928b266c 177
jordymorsinkhof 1:d4fa7e5e42cc 178 if (q1set>(0.75f*pi)) {q1set=(0.75f*pi);}
jordymorsinkhof 1:d4fa7e5e42cc 179 if (q1set<0.30f) {q1set = 0.30f;}
jordymorsinkhof 1:d4fa7e5e42cc 180 if (q2set>0.0f) {q2set=(0.0f);}
jordymorsinkhof 1:d4fa7e5e42cc 181 if (q2set<-2.79f) {q2set = -2.79f;}
jordymorsinkhof 1:d4fa7e5e42cc 182
BramS23 0:df93928b266c 183 // Call the motor control functions
BramS23 0:df93928b266c 184 Motor1.GotoPos(q1set);
BramS23 0:df93928b266c 185 Motor2.GotoPos(q2set);
BramS23 0:df93928b266c 186 }
BramS23 0:df93928b266c 187
BramS23 0:df93928b266c 188
BramS23 0:df93928b266c 189 // Start homing the motors
BramS23 0:df93928b266c 190 void HomingLoop(){
BramS23 0:df93928b266c 191 // with param:(Direction , PWM , Home pos in radians)
jordymorsinkhof 1:d4fa7e5e42cc 192 ZServo.write(0.1f); // Varying the servo pwm between 2.5 and 10 % sends it between 0 and 180 degrees
jordymorsinkhof 1:d4fa7e5e42cc 193 wait(1);
jordymorsinkhof 1:d4fa7e5e42cc 194 Motor1.Homing(1,0.1f,0.2879f);
jordymorsinkhof 1:d4fa7e5e42cc 195 Motor2.Homing(1,0.15f,-2.897f);
BramS23 0:df93928b266c 196 }
BramS23 0:df93928b266c 197
BramS23 0:df93928b266c 198 // Function for picking up a checkers playthingy
BramS23 0:df93928b266c 199 void PickUp(){
jordymorsinkhof 1:d4fa7e5e42cc 200 ZServo.write(0.035f); // Varying the servo pwm between 2.5 and 10 % sends it between 0 and 180 degrees
jordymorsinkhof 1:d4fa7e5e42cc 201 wait(1);
jordymorsinkhof 1:d4fa7e5e42cc 202 Magnet=1;
jordymorsinkhof 1:d4fa7e5e42cc 203 ZServo.write(0.1f);
jordymorsinkhof 1:d4fa7e5e42cc 204 wait(1);
BramS23 0:df93928b266c 205 }
BramS23 0:df93928b266c 206
BramS23 0:df93928b266c 207 // Function for dropping a checkers playthingy
BramS23 0:df93928b266c 208 void LayDown(){
jordymorsinkhof 1:d4fa7e5e42cc 209 //ZServo = 0.0f;
jordymorsinkhof 1:d4fa7e5e42cc 210 ZServo.write(0.035f); // Varying the servo pwm between 2.5 and 10 % sends it between 0 and 180 degrees
jordymorsinkhof 1:d4fa7e5e42cc 211 wait(1);
jordymorsinkhof 1:d4fa7e5e42cc 212 Magnet=0;
jordymorsinkhof 1:d4fa7e5e42cc 213
jordymorsinkhof 1:d4fa7e5e42cc 214 ZServo.write(0.1f);
jordymorsinkhof 1:d4fa7e5e42cc 215 wait(1);
BramS23 0:df93928b266c 216 }
BramS23 0:df93928b266c 217
BramS23 0:df93928b266c 218 // Forward declarate remote controller function
BramS23 0:df93928b266c 219 void RemoteController();
BramS23 0:df93928b266c 220
BramS23 0:df93928b266c 221
BramS23 0:df93928b266c 222 // Give Reference Position
BramS23 0:df93928b266c 223 void DeterminePosRef(){
jordymorsinkhof 1:d4fa7e5e42cc 224 //GXset=40*PotMeter1.read(); // Reference in Rad
jordymorsinkhof 1:d4fa7e5e42cc 225 //GYset=40*PotMeter2.read(); // Reference in Rad
jordymorsinkhof 1:d4fa7e5e42cc 226 //printf("Set: %f %f",GXset,GYset);
BramS23 0:df93928b266c 227 }
BramS23 0:df93928b266c 228
BramS23 0:df93928b266c 229 int main() {
jordymorsinkhof 1:d4fa7e5e42cc 230 //ZServo.calibrate(0.0005,120);
jordymorsinkhof 1:d4fa7e5e42cc 231 ZServo.period_ms(20); // Servo pwm interval should be 50 Hz
BramS23 0:df93928b266c 232 pc.baud(115200);
BramS23 0:df93928b266c 233 pc.printf("Program BIOROBOTICS startup\r\n");
jordymorsinkhof 1:d4fa7e5e42cc 234 q1set = 0.25f*pi;
jordymorsinkhof 1:d4fa7e5e42cc 235 q2set = -0.75f*pi;
BramS23 0:df93928b266c 236
BramS23 0:df93928b266c 237 // Define Controller Values
BramS23 0:df93928b266c 238 Motor1.SetInputLimits(-2.0f*pi,2.0f*pi);
jordymorsinkhof 1:d4fa7e5e42cc 239 Motor2.SetInputLimits(-2.0f*pi,2.0f*pi);
BramS23 0:df93928b266c 240
jordymorsinkhof 1:d4fa7e5e42cc 241 Motor1.SetOutputLimits(-0.3f,0.3f);
jordymorsinkhof 1:d4fa7e5e42cc 242 Motor2.SetOutputLimits(-0.8f,0.8f);
BramS23 0:df93928b266c 243
jordymorsinkhof 1:d4fa7e5e42cc 244 Motor1.SetPID(50.0f,10.0f,0.001f);
jordymorsinkhof 1:d4fa7e5e42cc 245 Motor2.SetPID(50.0f,10.0f,0.001f);
BramS23 0:df93928b266c 246
BramS23 0:df93928b266c 247 Motor1.SetGearRatio(3.0f);
BramS23 0:df93928b266c 248 Motor2.SetGearRatio(1.8f);
BramS23 0:df93928b266c 249
BramS23 0:df93928b266c 250 // Start homing function
BramS23 0:df93928b266c 251 pc.printf("Homing \r\n");
BramS23 0:df93928b266c 252 HomingLoop();
BramS23 0:df93928b266c 253
BramS23 0:df93928b266c 254 // Start Tickers
BramS23 0:df93928b266c 255 pc.printf("Starting Tickers \r\n");
jordymorsinkhof 1:d4fa7e5e42cc 256 ControlTicker.attach(&LoopJacInverse,Interval);
BramS23 0:df93928b266c 257 DirectionButton.rise(&DirectionButtonPressed);
BramS23 0:df93928b266c 258
BramS23 0:df93928b266c 259 // Check wheater a remote command has been send
BramS23 0:df93928b266c 260 while(1)
BramS23 0:df93928b266c 261 {
BramS23 0:df93928b266c 262 if (ir_rx.getState() == ReceiverIR::Received)
BramS23 0:df93928b266c 263 {
BramS23 0:df93928b266c 264 pc.printf("received");
BramS23 0:df93928b266c 265
BramS23 0:df93928b266c 266
BramS23 0:df93928b266c 267 RemoteController();
BramS23 0:df93928b266c 268 wait(0.1);
BramS23 0:df93928b266c 269 }
BramS23 0:df93928b266c 270 }
BramS23 0:df93928b266c 271
BramS23 0:df93928b266c 272
BramS23 0:df93928b266c 273 while(1){}
BramS23 0:df93928b266c 274
BramS23 0:df93928b266c 275
BramS23 0:df93928b266c 276 }
BramS23 0:df93928b266c 277
BramS23 0:df93928b266c 278
BramS23 0:df93928b266c 279
BramS23 0:df93928b266c 280 void RemoteController(){
BramS23 0:df93928b266c 281
BramS23 0:df93928b266c 282 int bitcount;
BramS23 0:df93928b266c 283
BramS23 0:df93928b266c 284
BramS23 0:df93928b266c 285 bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
BramS23 0:df93928b266c 286 int state = buf[2];
BramS23 0:df93928b266c 287 switch(state)
BramS23 0:df93928b266c 288 {
BramS23 0:df93928b266c 289 case 22: //1
BramS23 0:df93928b266c 290 pc.printf("1\n\r");
jordymorsinkhof 1:d4fa7e5e42cc 291 ControlTicker.attach(&LoopJacInverse,Interval);
jordymorsinkhof 1:d4fa7e5e42cc 292 break;
jordymorsinkhof 1:d4fa7e5e42cc 293
BramS23 0:df93928b266c 294 case 25: //1
BramS23 0:df93928b266c 295 pc.printf("2\n\r");
BramS23 0:df93928b266c 296 ControlTicker.detach();
BramS23 0:df93928b266c 297 Motor1.Stop();
BramS23 0:df93928b266c 298 Motor2.Stop();
BramS23 0:df93928b266c 299 PickUp();
jordymorsinkhof 1:d4fa7e5e42cc 300 ControlTicker.attach(&LoopJacInverse,Interval);
jordymorsinkhof 1:d4fa7e5e42cc 301 break;
jordymorsinkhof 1:d4fa7e5e42cc 302
jordymorsinkhof 1:d4fa7e5e42cc 303 case 13: //1
jordymorsinkhof 1:d4fa7e5e42cc 304 pc.printf("3\n\r");
jordymorsinkhof 1:d4fa7e5e42cc 305
jordymorsinkhof 1:d4fa7e5e42cc 306 break;
jordymorsinkhof 1:d4fa7e5e42cc 307
jordymorsinkhof 1:d4fa7e5e42cc 308 case 12: //1
jordymorsinkhof 1:d4fa7e5e42cc 309 pc.printf("4\n\r");
jordymorsinkhof 1:d4fa7e5e42cc 310 ControlTicker.detach();
jordymorsinkhof 1:d4fa7e5e42cc 311 Motor1.Stop();
jordymorsinkhof 1:d4fa7e5e42cc 312 Motor2.Stop();
jordymorsinkhof 1:d4fa7e5e42cc 313 GotoPosition(0.0f,0.0f);
jordymorsinkhof 1:d4fa7e5e42cc 314 break;
jordymorsinkhof 1:d4fa7e5e42cc 315
jordymorsinkhof 1:d4fa7e5e42cc 316 case 24: //1
jordymorsinkhof 1:d4fa7e5e42cc 317 pc.printf("5\n\r");
BramS23 0:df93928b266c 318 break;
BramS23 0:df93928b266c 319 case 94: //1
BramS23 0:df93928b266c 320 pc.printf("6\n\r");
jordymorsinkhof 1:d4fa7e5e42cc 321 ControlTicker.detach();
jordymorsinkhof 1:d4fa7e5e42cc 322 Motor1.Stop();
jordymorsinkhof 1:d4fa7e5e42cc 323 Motor2.Stop();
BramS23 0:df93928b266c 324 break;
BramS23 0:df93928b266c 325 case 8: //1
BramS23 0:df93928b266c 326 pc.printf("7\n\r");
BramS23 0:df93928b266c 327 break;
BramS23 0:df93928b266c 328 case 28: //1
BramS23 0:df93928b266c 329 pc.printf("8\n\r");
BramS23 0:df93928b266c 330 ControlTicker.detach();
BramS23 0:df93928b266c 331 Motor1.Stop();
BramS23 0:df93928b266c 332 Motor2.Stop();
BramS23 0:df93928b266c 333 LayDown();
jordymorsinkhof 1:d4fa7e5e42cc 334 ControlTicker.attach(&LoopJacInverse,Interval);
BramS23 0:df93928b266c 335 break;
BramS23 0:df93928b266c 336 case 90: //1
BramS23 0:df93928b266c 337 pc.printf("9\n\r");
BramS23 0:df93928b266c 338 break;
BramS23 0:df93928b266c 339 case 70: //1
BramS23 0:df93928b266c 340 pc.printf("Boven\n\r");
jordymorsinkhof 1:d4fa7e5e42cc 341 y_dot_remote = 5.0f;
BramS23 0:df93928b266c 342 break;
BramS23 0:df93928b266c 343 case 21: //1
BramS23 0:df93928b266c 344 pc.printf("Onder\n\r");
jordymorsinkhof 1:d4fa7e5e42cc 345 y_dot_remote = -5.0f;
BramS23 0:df93928b266c 346 break;
BramS23 0:df93928b266c 347 case 68: //1
BramS23 0:df93928b266c 348 pc.printf("Links\n\r");
jordymorsinkhof 1:d4fa7e5e42cc 349 x_dot_remote = -5.0f;
BramS23 0:df93928b266c 350 break;
BramS23 0:df93928b266c 351 case 67: //1
BramS23 0:df93928b266c 352 pc.printf("Rechts\n\r");
jordymorsinkhof 1:d4fa7e5e42cc 353 x_dot_remote = 5.0f;
BramS23 0:df93928b266c 354 break;
BramS23 0:df93928b266c 355 case 64: //1
BramS23 0:df93928b266c 356 pc.printf("OK\n\r");
jordymorsinkhof 1:d4fa7e5e42cc 357 x_dot_remote = 0.0f;
jordymorsinkhof 1:d4fa7e5e42cc 358 y_dot_remote = 0.0f;
BramS23 0:df93928b266c 359 break;
BramS23 0:df93928b266c 360 default:
BramS23 0:df93928b266c 361 break;
BramS23 0:df93928b266c 362 }
BramS23 0:df93928b266c 363 }