Floris Hoek / Mbed 2 deprecated template_biorobotics_Group_18

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@21:245c676f9d72, 2019-10-29 (annotated)

Committer:
paulstuiver
Date:
Tue Oct 29 14:55:38 2019 +0000
Revision:
21:245c676f9d72
Parent:
20:e20c26a1f6ba 
Circles by post malone

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
RobertoO 1:b862262a9d14 2 #include "MODSERIAL.h"
paulstuiver 2:75b2f713161c 3 #include "FastPWM.h"
paulstuiver 5:2ae500da8fe1 4 #include "QEI.h"
paulstuiver 5:2ae500da8fe1 5 #include <math.h>
paulstuiver 6:ea3b3f50c893 6 #include "BiQuad.h"
paulstuiver 2:75b2f713161c 7
paulstuiver 16:29d3851cfd52 8 // To play with buttons and potmeters
paulstuiver 2:75b2f713161c 9 AnalogIn pot2(A0);
paulstuiver 7:08fd3bc7a3cf 10 AnalogIn pot1(A1);
paulstuiver 20:e20c26a1f6ba 11 DigitalIn but1(D2);
paulstuiver 20:e20c26a1f6ba 12 DigitalIn but2(D3);
paulstuiver 20:e20c26a1f6ba 13 DigitalIn but3(SW2);
paulstuiver 20:e20c26a1f6ba 14
paulstuiver 16:29d3851cfd52 15
paulstuiver 16:29d3851cfd52 16 // Usual stuff
paulstuiver 6:ea3b3f50c893 17 MODSERIAL pc(USBTX, USBRX);
paulstuiver 16:29d3851cfd52 18
paulstuiver 16:29d3851cfd52 19 //ticker to call motor values
paulstuiver 16:29d3851cfd52 20 Ticker motor;
paulstuiver 16:29d3851cfd52 21
paulstuiver 16:29d3851cfd52 22 // Direction and Velocity of the motors
paulstuiver 16:29d3851cfd52 23 DigitalOut motor1Direction(D7);
paulstuiver 16:29d3851cfd52 24 FastPWM motor1Velocity(D6);
paulstuiver 16:29d3851cfd52 25 DigitalOut motor2Direction(D4);
paulstuiver 16:29d3851cfd52 26 FastPWM motor2Velocity(D5);
paulstuiver 16:29d3851cfd52 27
paulstuiver 16:29d3851cfd52 28 // Encoders 1 and 2 respectively
paulstuiver 16:29d3851cfd52 29 QEI Encoder1(D8,D9,NC,8400);
paulstuiver 16:29d3851cfd52 30 QEI Encoder2(D11,D10,NC,8400);
paulstuiver 7:08fd3bc7a3cf 31
paulstuiver 7:08fd3bc7a3cf 32
paulstuiver 2:75b2f713161c 33 volatile double frequency = 17000.0;
paulstuiver 2:75b2f713161c 34 volatile double period_signal = 1.0/frequency;
paulstuiver 7:08fd3bc7a3cf 35 float timeinterval = 0.001f;
paulstuiver 16:29d3851cfd52 36 float motorvalue1;
paulstuiver 16:29d3851cfd52 37 float motorvalue2;
paulstuiver 16:29d3851cfd52 38 float yendeffector = 10.6159;
paulstuiver 11:ca91fc47ad02 39 float xendeffector = 0;
paulstuiver 20:e20c26a1f6ba 40 float potmeter1 = 5+pot1.read()*60;
paulstuiver 20:e20c26a1f6ba 41 float potmeter2 = pot2.read()*60;
paulstuiver 21:245c676f9d72 42 float desiredmotorangle1;
paulstuiver 21:245c676f9d72 43 float desiredmotorangle2;
paulstuiver 11:ca91fc47ad02 44
paulstuiver 16:29d3851cfd52 45 // ANTI WIND UP NEEDED
paulstuiver 6:ea3b3f50c893 46
paulstuiver 6:ea3b3f50c893 47 // -------------------- README ------------------------------------
paulstuiver 6:ea3b3f50c893 48 // positive referenceposition corresponds to a counterclockwise motion
paulstuiver 6:ea3b3f50c893 49 // negative referenceposition corresponds to a clockwise motion
paulstuiver 6:ea3b3f50c893 50
paulstuiver 6:ea3b3f50c893 51 //PID control implementation (BiQuead)
paulstuiver 16:29d3851cfd52 52 double PID_controller1(float error1)
paulstuiver 5:2ae500da8fe1 53 {
paulstuiver 16:29d3851cfd52 54 // Define errors for motor 1 and 2
paulstuiver 16:29d3851cfd52 55 static double error_integral1 = 0;
paulstuiver 16:29d3851cfd52 56 static double error_prev1 = error1;
paulstuiver 16:29d3851cfd52 57
paulstuiver 16:29d3851cfd52 58 // Low-pass filter
paulstuiver 6:ea3b3f50c893 59 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
paulstuiver 16:29d3851cfd52 60
paulstuiver 16:29d3851cfd52 61 // PID variables: we assume them to be the same for both motors
paulstuiver 20:e20c26a1f6ba 62 float Kp = 64.9;
paulstuiver 20:e20c26a1f6ba 63 float Ki = 3.64;
paulstuiver 20:e20c26a1f6ba 64 float Kd = 5;
paulstuiver 6:ea3b3f50c893 65
paulstuiver 5:2ae500da8fe1 66 //Proportional part:
paulstuiver 16:29d3851cfd52 67 double u_k1 = Kp * error1;
paulstuiver 5:2ae500da8fe1 68
paulstuiver 6:ea3b3f50c893 69 // Integreal part
paulstuiver 16:29d3851cfd52 70 error_integral1 = error_integral1 + error1 * timeinterval;
paulstuiver 16:29d3851cfd52 71 double u_i1 = Ki*error_integral1;
paulstuiver 16:29d3851cfd52 72
paulstuiver 6:ea3b3f50c893 73 // Derivate part
paulstuiver 16:29d3851cfd52 74 double error_derivative1 = (error1 - error_prev1)/timeinterval;
paulstuiver 16:29d3851cfd52 75 double filtered_error_derivative1 = LowPassFilter.step(error_derivative1);
paulstuiver 16:29d3851cfd52 76 double u_d1 = Kd * filtered_error_derivative1;
paulstuiver 16:29d3851cfd52 77 error_prev1 = error1;
paulstuiver 6:ea3b3f50c893 78
paulstuiver 5:2ae500da8fe1 79 //sum all parts and return it
paulstuiver 16:29d3851cfd52 80 return u_k1 + u_i1 + u_d1;
paulstuiver 16:29d3851cfd52 81 }
paulstuiver 16:29d3851cfd52 82
paulstuiver 16:29d3851cfd52 83 double PID_controller2(float error2)
paulstuiver 16:29d3851cfd52 84 {
paulstuiver 16:29d3851cfd52 85 // Define errors for motor 1 and 2
paulstuiver 16:29d3851cfd52 86 static double error_integral2 = 0;
paulstuiver 16:29d3851cfd52 87 static double error_prev2 = error2;
paulstuiver 16:29d3851cfd52 88
paulstuiver 16:29d3851cfd52 89 // Low-pass filter
paulstuiver 16:29d3851cfd52 90 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
paulstuiver 16:29d3851cfd52 91
paulstuiver 16:29d3851cfd52 92 // PID variables: we assume them to be the same for both motors
paulstuiver 20:e20c26a1f6ba 93 float Kp = 64.9;
paulstuiver 20:e20c26a1f6ba 94 float Ki = 3.64;
paulstuiver 20:e20c26a1f6ba 95 float Kd = 5;
paulstuiver 16:29d3851cfd52 96
paulstuiver 16:29d3851cfd52 97 //Proportional part:
paulstuiver 16:29d3851cfd52 98 double u_k2 = Kp * error2;
paulstuiver 16:29d3851cfd52 99
paulstuiver 16:29d3851cfd52 100 // Integreal part
paulstuiver 16:29d3851cfd52 101 error_integral2 = error_integral2 + error2 * timeinterval;
paulstuiver 16:29d3851cfd52 102 double u_i2 = Ki*error_integral2;
paulstuiver 16:29d3851cfd52 103
paulstuiver 16:29d3851cfd52 104 // Derivate part
paulstuiver 16:29d3851cfd52 105 double error_derivative2 = (error2 - error_prev2)/timeinterval;
paulstuiver 16:29d3851cfd52 106 double filtered_error_derivative2 = LowPassFilter.step(error_derivative2);
paulstuiver 16:29d3851cfd52 107 double u_d2 = Kd * filtered_error_derivative2;
paulstuiver 16:29d3851cfd52 108 error_prev2 = error2;
paulstuiver 16:29d3851cfd52 109
paulstuiver 16:29d3851cfd52 110 //sum all parts and return it
paulstuiver 16:29d3851cfd52 111 return u_k2 + u_i2 + u_d2;
paulstuiver 5:2ae500da8fe1 112 }
paulstuiver 2:75b2f713161c 113
paulstuiver 6:ea3b3f50c893 114 //get the measured position
paulstuiver 16:29d3851cfd52 115 void getmeasuredposition(float & measuredposition1, float & measuredposition2)
paulstuiver 2:75b2f713161c 116 {
paulstuiver 16:29d3851cfd52 117 // Obtain the counts of motors 1 and 2 from the encoder
paulstuiver 16:29d3851cfd52 118 int countsmotor1;
paulstuiver 16:29d3851cfd52 119 int countsmotor2;
paulstuiver 16:29d3851cfd52 120 countsmotor1 = Encoder1.getPulses();
paulstuiver 16:29d3851cfd52 121 countsmotor2 = Encoder2.getPulses();
paulstuiver 6:ea3b3f50c893 122
paulstuiver 16:29d3851cfd52 123 // Obtain the measured position for motor 1 and 2
paulstuiver 16:29d3851cfd52 124 measuredposition1 = ((float)countsmotor1) / 8400.0f * 2.0f;
paulstuiver 16:29d3851cfd52 125 measuredposition2 = ((float)countsmotor2) / 8400.0f * 2.0f;
paulstuiver 2:75b2f713161c 126 }
paulstuiver 2:75b2f713161c 127
paulstuiver 16:29d3851cfd52 128 //get the reference of the
paulstuiver 16:29d3851cfd52 129 void getreferenceposition(float & referenceposition1, float & referenceposition2)
paulstuiver 2:75b2f713161c 130 {
paulstuiver 16:29d3851cfd52 131 //Measurements of the arm
paulstuiver 16:29d3851cfd52 132 float L0=1.95;
paulstuiver 11:ca91fc47ad02 133 float L1=15;
paulstuiver 11:ca91fc47ad02 134 float L2=20;
paulstuiver 16:29d3851cfd52 135
paulstuiver 16:29d3851cfd52 136 //Define the new counts that are needed
paulstuiver 21:245c676f9d72 137 // float desiredmotorangle1;
paulstuiver 21:245c676f9d72 138 // float desiredmotorangle2;
paulstuiver 16:29d3851cfd52 139
paulstuiver 16:29d3851cfd52 140 //Inverse kinematics: given the end position, what are the desired motor angles of 1 and 2
paulstuiver 21:245c676f9d72 141 desiredmotorangle1 = (atan2(yendeffector,(L0+xendeffector))*180/3.1415 + acos((pow(L1,2)+pow(L0+xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0+xendeffector,2)+pow(yendeffector,2))))*180/3.1415)-180;
paulstuiver 21:245c676f9d72 142 desiredmotorangle2 = (atan2(yendeffector,(L0-xendeffector))*180/3.1415 + acos((pow(L1,2)+pow(L0-xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0-xendeffector,2)+pow(yendeffector,2))))*180/3.1415)-180;
paulstuiver 16:29d3851cfd52 143
paulstuiver 21:245c676f9d72 144 //Apply boundaries of the motor angles
paulstuiver 21:245c676f9d72 145 // if (desiredmotorangle1 < -125){
paulstuiver 21:245c676f9d72 146 // desiredmotorangle1 = -124.9;
paulstuiver 21:245c676f9d72 147 // }
paulstuiver 21:245c676f9d72 148 // if (desiredmotorangle1 > 28){
paulstuiver 21:245c676f9d72 149 // desiredmotorangle1 = 27.9;
paulstuiver 21:245c676f9d72 150 // }
paulstuiver 21:245c676f9d72 151 // if (desiredmotorangle2 > 46){
paulstuiver 21:245c676f9d72 152 // desiredmotorangle2 = 45.9;
paulstuiver 21:245c676f9d72 153 // }
paulstuiver 21:245c676f9d72 154 // if (desiredmotorangle2 < -130){
paulstuiver 21:245c676f9d72 155 // desiredmotorangle2 = -129.9;
paulstuiver 21:245c676f9d72 156 // }
paulstuiver 21:245c676f9d72 157
paulstuiver 16:29d3851cfd52 158 //Convert motor angles to counts
paulstuiver 16:29d3851cfd52 159 float desiredmotorrounds1;
paulstuiver 16:29d3851cfd52 160 float desiredmotorrounds2;
paulstuiver 21:245c676f9d72 161 desiredmotorrounds1 = (desiredmotorangle1)/360;
paulstuiver 21:245c676f9d72 162 desiredmotorrounds2 = (desiredmotorangle2)/360;
paulstuiver 16:29d3851cfd52 163
paulstuiver 16:29d3851cfd52 164 //Assign this to new variables because otherwise it doesn't work
paulstuiver 16:29d3851cfd52 165 referenceposition1 = desiredmotorrounds1;
paulstuiver 16:29d3851cfd52 166 referenceposition2 = desiredmotorrounds2;
paulstuiver 2:75b2f713161c 167 }
RobertoO 0:67c50348f842 168
paulstuiver 2:75b2f713161c 169 //send value to motor
paulstuiver 16:29d3851cfd52 170 void sendtomotor(float & motorvalue1, float & motorvalue2)
paulstuiver 2:75b2f713161c 171 {
paulstuiver 16:29d3851cfd52 172 // Define the absolute motor values
paulstuiver 16:29d3851cfd52 173 float absolutemotorvalue1 = 0.0f;
paulstuiver 16:29d3851cfd52 174 float absolutemotorvalue2 = 0.0f;
paulstuiver 16:29d3851cfd52 175 absolutemotorvalue1 = fabs(motorvalue1);
paulstuiver 16:29d3851cfd52 176 absolutemotorvalue2 = fabs(motorvalue2);
paulstuiver 16:29d3851cfd52 177
paulstuiver 16:29d3851cfd52 178 // If absolutemotorvalueocity is greater than 1, reduce to 1, otherwise remain absolutemotorvalue
paulstuiver 16:29d3851cfd52 179 absolutemotorvalue1 = absolutemotorvalue1 > 1.0f ? 1.0f : absolutemotorvalue1;
paulstuiver 16:29d3851cfd52 180 absolutemotorvalue2 = absolutemotorvalue2 > 1.0f ? 1.0f : absolutemotorvalue2;
paulstuiver 16:29d3851cfd52 181
paulstuiver 16:29d3851cfd52 182 // Send the absolutemotorvalue to the motors
paulstuiver 16:29d3851cfd52 183 motor1Velocity = absolutemotorvalue1;
paulstuiver 16:29d3851cfd52 184 motor2Velocity = absolutemotorvalue2;
paulstuiver 16:29d3851cfd52 185
paulstuiver 16:29d3851cfd52 186 // Determine the motor direction. Boolean output: true gives counterclockwise direction, false gives clockwise direction
paulstuiver 16:29d3851cfd52 187 motor1Direction = (motorvalue1 > 0.0f);
paulstuiver 16:29d3851cfd52 188 motor2Direction = (motorvalue2 > 0.0f);
paulstuiver 2:75b2f713161c 189 }
RobertoO 0:67c50348f842 190
paulstuiver 7:08fd3bc7a3cf 191 // function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
paulstuiver 2:75b2f713161c 192 void measureandcontrol(void)
paulstuiver 2:75b2f713161c 193 {
paulstuiver 16:29d3851cfd52 194 // Get the reference positions of motor 1 and 2
paulstuiver 16:29d3851cfd52 195 float reference1, reference2;
paulstuiver 16:29d3851cfd52 196 getreferenceposition(reference1, reference2);
paulstuiver 16:29d3851cfd52 197
paulstuiver 16:29d3851cfd52 198 // Get the measured positions of motor 1 and 2
paulstuiver 16:29d3851cfd52 199 float measured1, measured2;
paulstuiver 16:29d3851cfd52 200 getmeasuredposition(measured1, measured2);
paulstuiver 16:29d3851cfd52 201
paulstuiver 16:29d3851cfd52 202 // Calculate the motor values
paulstuiver 16:29d3851cfd52 203 float motorvalue1, motorvalue2;
paulstuiver 16:29d3851cfd52 204 motorvalue1 = PID_controller1(reference1 - measured1);
paulstuiver 16:29d3851cfd52 205 motorvalue2 = PID_controller2(reference2 - measured2);
paulstuiver 16:29d3851cfd52 206 sendtomotor(motorvalue1, motorvalue2);
paulstuiver 2:75b2f713161c 207 }
RobertoO 0:67c50348f842 208 int main()
RobertoO 0:67c50348f842 209 {
paulstuiver 16:29d3851cfd52 210 // Usual stuff
RobertoO 0:67c50348f842 211 pc.baud(115200);
paulstuiver 2:75b2f713161c 212 pc.printf("Starting...\r\n");
paulstuiver 16:29d3851cfd52 213
paulstuiver 16:29d3851cfd52 214 // Something with pulses
paulstuiver 16:29d3851cfd52 215 motor1Velocity.period(period_signal);
paulstuiver 16:29d3851cfd52 216 motor2Velocity.period(period_signal);
paulstuiver 16:29d3851cfd52 217
paulstuiver 16:29d3851cfd52 218 // Call the ticker function
paulstuiver 6:ea3b3f50c893 219 motor.attach(measureandcontrol, timeinterval);
paulstuiver 2:75b2f713161c 220 while(true)
paulstuiver 2:75b2f713161c 221 {
paulstuiver 20:e20c26a1f6ba 222 wait(0.01);
paulstuiver 20:e20c26a1f6ba 223 float potmeter1 = 5+pot1.read()*60;
paulstuiver 20:e20c26a1f6ba 224 float potmeter2 = pot2.read()*60;
paulstuiver 21:245c676f9d72 225 // pc.printf("Kp gives %f\r\n", potmeter1);
paulstuiver 21:245c676f9d72 226 // pc.printf("Ki gives %f\r\n", potmeter2);
paulstuiver 11:ca91fc47ad02 227 pc.printf("x is %f\r\n",xendeffector);
paulstuiver 21:245c676f9d72 228 pc.printf("y is %f\r\n",yendeffector);
paulstuiver 21:245c676f9d72 229 pc.printf("motorangle1 is%f\r\n",desiredmotorangle1);
paulstuiver 21:245c676f9d72 230 pc.printf("motorangle2 is%f\r\n",desiredmotorangle2);
paulstuiver 11:ca91fc47ad02 231
paulstuiver 21:245c676f9d72 232 float ditmoetkleinerzijndan = pow((xendeffector-1.95),2)+pow(yendeffector,2);
paulstuiver 21:245c676f9d72 233 float dit = 28*28;
paulstuiver 21:245c676f9d72 234
paulstuiver 21:245c676f9d72 235 float ditmoetkleinerzijndan2 = pow((xendeffector-1.95),2)+pow(yendeffector,2);
paulstuiver 21:245c676f9d72 236 float dit2 = 28*28;
paulstuiver 21:245c676f9d72 237
paulstuiver 21:245c676f9d72 238
paulstuiver 21:245c676f9d72 239
paulstuiver 20:e20c26a1f6ba 240 // Control position
paulstuiver 20:e20c26a1f6ba 241 char a = pc.getcNb();
paulstuiver 21:245c676f9d72 242 if(xendeffector > 1.95){
paulstuiver 21:245c676f9d72 243 if(a == 'r' && ditmoetkleinerzijndan < dit){
paulstuiver 21:245c676f9d72 244 xendeffector=xendeffector+1;}
paulstuiver 21:245c676f9d72 245 else if(a=='l'){
paulstuiver 21:245c676f9d72 246 xendeffector=xendeffector-1;}
paulstuiver 21:245c676f9d72 247 else if(a=='u' && ditmoetkleinerzijndan2 < dit2){
paulstuiver 21:245c676f9d72 248 yendeffector=yendeffector+1;}
paulstuiver 21:245c676f9d72 249 else if(a=='d'){
paulstuiver 21:245c676f9d72 250 yendeffector=yendeffector-1;}
paulstuiver 21:245c676f9d72 251 }
paulstuiver 21:245c676f9d72 252 else if( 0 < xendeffector < 1.95){
paulstuiver 21:245c676f9d72 253 if(a == 'r'){
paulstuiver 21:245c676f9d72 254 xendeffector=xendeffector+1;}
paulstuiver 21:245c676f9d72 255 else if(a=='l' && pow(xendeffector-1.95,2)+pow(yendeffector,2) < 28*28){
paulstuiver 21:245c676f9d72 256 xendeffector=xendeffector-1;}
paulstuiver 21:245c676f9d72 257 else if(a=='u' && pow(xendeffector-1.95,2)+pow(yendeffector,2) < 28*28){
paulstuiver 21:245c676f9d72 258 yendeffector=yendeffector+1;}
paulstuiver 21:245c676f9d72 259 else if(a=='d'){
paulstuiver 21:245c676f9d72 260 yendeffector=yendeffector-1;}
paulstuiver 21:245c676f9d72 261 }
paulstuiver 21:245c676f9d72 262 else if( -1.95 < xendeffector < 0){
paulstuiver 21:245c676f9d72 263 if(a == 'r' && pow(xendeffector+1.95,2)+pow(yendeffector,2) < 28*28){
paulstuiver 21:245c676f9d72 264 xendeffector=xendeffector+1;}
paulstuiver 21:245c676f9d72 265 else if(a=='l'){
paulstuiver 21:245c676f9d72 266 xendeffector=xendeffector-1;}
paulstuiver 21:245c676f9d72 267 else if(a=='u' && pow(xendeffector+1.95,2)+pow(yendeffector,2) < 28*28){
paulstuiver 21:245c676f9d72 268 yendeffector=yendeffector+1;}
paulstuiver 21:245c676f9d72 269 else if(a=='d'){
paulstuiver 21:245c676f9d72 270 yendeffector=yendeffector-1;}
paulstuiver 21:245c676f9d72 271 }
paulstuiver 21:245c676f9d72 272 else if( xendeffector < -1.95){
paulstuiver 21:245c676f9d72 273 if(a == 'r'){
paulstuiver 21:245c676f9d72 274 xendeffector=xendeffector+1;}
paulstuiver 21:245c676f9d72 275 else if(a=='l' && pow(xendeffector+1.95,2)+pow(yendeffector,2) < 28*28){
paulstuiver 21:245c676f9d72 276 xendeffector=xendeffector-1;}
paulstuiver 21:245c676f9d72 277 else if(a=='u' && pow(xendeffector+1.95,2)+pow(yendeffector,2) < 28*28){
paulstuiver 21:245c676f9d72 278 yendeffector=yendeffector+1;}
paulstuiver 21:245c676f9d72 279 else if(a=='d'){
paulstuiver 21:245c676f9d72 280 yendeffector=yendeffector-1;}
paulstuiver 21:245c676f9d72 281 }
paulstuiver 20:e20c26a1f6ba 282
paulstuiver 20:e20c26a1f6ba 283 // Go back to starting position
paulstuiver 20:e20c26a1f6ba 284 if (but1.read() == 0) {
paulstuiver 20:e20c26a1f6ba 285 xendeffector=0;
paulstuiver 20:e20c26a1f6ba 286 yendeffector=10.6159;
paulstuiver 20:e20c26a1f6ba 287 }
paulstuiver 20:e20c26a1f6ba 288
paulstuiver 20:e20c26a1f6ba 289 if (but2.read() == 0){
paulstuiver 20:e20c26a1f6ba 290 xendeffector=-5;
paulstuiver 20:e20c26a1f6ba 291 yendeffector=10.6159;
paulstuiver 20:e20c26a1f6ba 292 wait(0.5);
paulstuiver 20:e20c26a1f6ba 293
paulstuiver 20:e20c26a1f6ba 294 xendeffector=10;
paulstuiver 20:e20c26a1f6ba 295 yendeffector=25.6159;
paulstuiver 20:e20c26a1f6ba 296 wait(0.5);
paulstuiver 20:e20c26a1f6ba 297
paulstuiver 20:e20c26a1f6ba 298 xendeffector=-14;
paulstuiver 20:e20c26a1f6ba 299 yendeffector=21.6159;
paulstuiver 20:e20c26a1f6ba 300 wait(0.5);
paulstuiver 20:e20c26a1f6ba 301
paulstuiver 20:e20c26a1f6ba 302 xendeffector=10;
paulstuiver 20:e20c26a1f6ba 303 yendeffector=11.6159;
paulstuiver 20:e20c26a1f6ba 304 wait(0.5);
paulstuiver 20:e20c26a1f6ba 305
paulstuiver 20:e20c26a1f6ba 306 xendeffector=0;
paulstuiver 20:e20c26a1f6ba 307 yendeffector=10.6159;
paulstuiver 20:e20c26a1f6ba 308 wait(0.5);
paulstuiver 20:e20c26a1f6ba 309 }
paulstuiver 20:e20c26a1f6ba 310
paulstuiver 20:e20c26a1f6ba 311 if (but3.read() ==0){
paulstuiver 20:e20c26a1f6ba 312
paulstuiver 20:e20c26a1f6ba 313 xendeffector=-5;
paulstuiver 20:e20c26a1f6ba 314 yendeffector=10.6159;
paulstuiver 20:e20c26a1f6ba 315 int i=0;
paulstuiver 20:e20c26a1f6ba 316 wait(0.5);
paulstuiver 20:e20c26a1f6ba 317 while(i<100){
paulstuiver 20:e20c26a1f6ba 318 xendeffector=xendeffector+0.1;
paulstuiver 20:e20c26a1f6ba 319 yendeffector=yendeffector+0.1;
paulstuiver 20:e20c26a1f6ba 320 i++;
paulstuiver 20:e20c26a1f6ba 321 wait(0.001);
paulstuiver 20:e20c26a1f6ba 322 }
paulstuiver 20:e20c26a1f6ba 323 }
paulstuiver 20:e20c26a1f6ba 324
paulstuiver 20:e20c26a1f6ba 325
paulstuiver 11:ca91fc47ad02 326
paulstuiver 11:ca91fc47ad02 327
paulstuiver 20:e20c26a1f6ba 328 // type c to stop the program
paulstuiver 6:ea3b3f50c893 329 char c = pc.getcNb();
paulstuiver 6:ea3b3f50c893 330 if (c == 'c')
paulstuiver 6:ea3b3f50c893 331 {
paulstuiver 7:08fd3bc7a3cf 332 pc.printf("Program stopped running");
paulstuiver 16:29d3851cfd52 333 motorvalue1 = 0;
paulstuiver 16:29d3851cfd52 334 motorvalue2 = 0;
paulstuiver 16:29d3851cfd52 335 sendtomotor(motorvalue1, motorvalue2);
paulstuiver 7:08fd3bc7a3cf 336 break;
paulstuiver 6:ea3b3f50c893 337 }
RobertoO 0:67c50348f842 338 }
paulstuiver 2:75b2f713161c 339 }