Clément Ballot / Mbed 2 deprecated BIXI_arm_2_axes

Dependencies:   Servo TextLCD mbed

main.cpp@3:dd4efdfbbf01, 2016-06-17 (annotated)

Committer:
clementb
Date:
Fri Jun 17 10:58:45 2016 +0000
Revision:
3:dd4efdfbbf01
Parent:
2:2728901fd798
Child:
4:1a6e5f38f0d9
Tested only with the X servo. But the rotation speed and the LCD work fine.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
clementb 2:2728901fd798 1 /*
clementb 2:2728901fd798 2 Program created in June, 2016 by Clément BALLOT
clementb 2:2728901fd798 3
clementb 2:2728901fd798 4 This program is for a robotic arm for demonstrates the Bixi capacities, with 2 axes. The Bixi is a Bluetooth module for touch-free control
clementb 2:2728901fd798 5 Bluetooth devices, like a smartphone or a tablet. It is develloped by Bluemint Labs.
clementb 2:2728901fd798 6
clementb 2:2728901fd798 7 To make this project, you need :
clementb 2:2728901fd798 8 - a STM32 Nucleo board (ST);
clementb 2:2728901fd798 9 - 2x servomotors (5V);
clementb 2:2728901fd798 10 - 2x LEDs : 1x red, 1x green;
clementb 2:2728901fd798 11 - 3 pushbuttons;
clementb 2:2728901fd798 12 - 4x 10K potentiometers;
clementb 2:2728901fd798 13 - a 2 rows 16 columns alphanumeric LCD screen;
clementb 2:2728901fd798 14 - a USB A to micro cable;
clementb 2:2728901fd798 15 - a breadboard;
clementb 2:2728901fd798 16 - 2x 100K resistors.
clementb 2:2728901fd798 17 */
clementb 0:9438fe754ab6 18 #include "mbed.h"
clementb 0:9438fe754ab6 19 #include "TextLCD.h"
clementb 0:9438fe754ab6 20 #include "Servo.h"
clementb 0:9438fe754ab6 21
clementb 0:9438fe754ab6 22
clementb 2:2728901fd798 23 TextLCD lcd(D2, D3, D4, D5, D6, D7);
clementb 2:2728901fd798 24 enum LCDType{LCD16x2B};
clementb 0:9438fe754ab6 25
clementb 2:2728901fd798 26 Servo servoRL(D10); //Righ-left servo : x axe
clementb 2:2728901fd798 27 Servo servoHL(D9); //Up-down servo : y axe
clementb 0:9438fe754ab6 28
clementb 2:2728901fd798 29 DigitalIn menuL(D11); //"Back" button
clementb 2:2728901fd798 30 DigitalIn menuR(D12); //"Next" button
clementb 2:2728901fd798 31 DigitalOut ledY(D8); //"Please wait" LED
clementb 2:2728901fd798 32 DigitalOut ledV(D13); //"In progress" and "press a button" LED
clementb 2:2728901fd798 33 AnalogIn analogX(A1); //Analog in for x potentiometer
clementb 2:2728901fd798 34 AnalogIn analogY(A3); //Analog in for y potentiometer
clementb 0:9438fe754ab6 35
clementb 3:dd4efdfbbf01 36 float vitesse = 0.001; //--------------------------------------------- CHANGE IT TO REDUCE/INCREASE SERVOMOTORS SPEED --------------------
clementb 0:9438fe754ab6 37 int page;
clementb 2:2728901fd798 38 int i = 0;
clementb 3:dd4efdfbbf01 39 int disp = 0;
clementb 3:dd4efdfbbf01 40 float xPos = 0;
clementb 3:dd4efdfbbf01 41 float yPos = 0;
clementb 0:9438fe754ab6 42
clementb 1:c3a33d9372f5 43
clementb 1:c3a33d9372f5 44 int main(){
clementb 1:c3a33d9372f5 45 page = 1;
clementb 0:9438fe754ab6 46 lcd.cls();
clementb 0:9438fe754ab6 47 lcd.locate(0,0);
clementb 2:2728901fd798 48 lcd.printf("BIXI Rob'Arm :by");
clementb 1:c3a33d9372f5 49 lcd.locate(0,1);
clementb 2:2728901fd798 50 lcd.printf("Clement BALLOT");
clementb 1:c3a33d9372f5 51 servoRL = 0;
clementb 1:c3a33d9372f5 52 servoHL = 0;
clementb 2:2728901fd798 53 while(i != 50){
clementb 2:2728901fd798 54 ledY = 1;
clementb 2:2728901fd798 55 ledV = 0;
clementb 1:c3a33d9372f5 56 wait(0.05);
clementb 2:2728901fd798 57 ledY = 0;
clementb 2:2728901fd798 58 ledV = 1;
clementb 1:c3a33d9372f5 59 wait(0.05);
clementb 2:2728901fd798 60 i = i++;
clementb 0:9438fe754ab6 61 }
clementb 2:2728901fd798 62 ledY = 0;
clementb 2:2728901fd798 63 ledV = 0;
clementb 0:9438fe754ab6 64
clementb 1:c3a33d9372f5 65 while(1) {
clementb 2:2728901fd798 66 ledY = 1;
clementb 2:2728901fd798 67 lcd.cls();
clementb 2:2728901fd798 68 lcd.locate(0,0);
clementb 2:2728901fd798 69 lcd.printf("Reset servo pos");
clementb 2:2728901fd798 70 wait(2);
clementb 2:2728901fd798 71 lcd.locate(0, 1);
clementb 2:2728901fd798 72 lcd.printf("Please wait...");
clementb 3:dd4efdfbbf01 73 wait(1);
clementb 2:2728901fd798 74 lcd.cls();
clementb 2:2728901fd798 75 lcd.locate(3, 0);
clementb 2:2728901fd798 76 lcd.printf(" : x servo");
clementb 2:2728901fd798 77 lcd.locate(3, 1);
clementb 2:2728901fd798 78 lcd.printf(" : y servo");
clementb 2:2728901fd798 79 lcd.locate(0,0);
clementb 3:dd4efdfbbf01 80 disp = xPos*100;
clementb 3:dd4efdfbbf01 81 lcd.printf("%3d", disp);
clementb 2:2728901fd798 82 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 83 disp = yPos*100;
clementb 3:dd4efdfbbf01 84 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 85 if(xPos!=0){
clementb 3:dd4efdfbbf01 86 while(yPos < 1.00){
clementb 3:dd4efdfbbf01 87 yPos = yPos + 0.01;
clementb 2:2728901fd798 88 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 89 disp = yPos*100;
clementb 3:dd4efdfbbf01 90 lcd.printf("%3d", disp);
clementb 2:2728901fd798 91 servoHL = yPos;
clementb 3:dd4efdfbbf01 92 wait(vitesse*10);
clementb 1:c3a33d9372f5 93 }
clementb 0:9438fe754ab6 94 }
clementb 3:dd4efdfbbf01 95 wait(1);
clementb 3:dd4efdfbbf01 96 while(xPos > 0.00){
clementb 2:2728901fd798 97 xPos = xPos - 0.01;
clementb 2:2728901fd798 98 lcd.locate(0,0);
clementb 3:dd4efdfbbf01 99 disp = xPos*100;
clementb 3:dd4efdfbbf01 100 lcd.printf("%3d", disp);
clementb 2:2728901fd798 101 servoRL = xPos;
clementb 3:dd4efdfbbf01 102 wait(vitesse*10);
clementb 2:2728901fd798 103 }
clementb 3:dd4efdfbbf01 104 wait(1);
clementb 3:dd4efdfbbf01 105 while(yPos >0.00){
clementb 3:dd4efdfbbf01 106 yPos = yPos - 0.01;
clementb 2:2728901fd798 107 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 108 disp = yPos*100;
clementb 3:dd4efdfbbf01 109 lcd.printf("%3d", disp);
clementb 2:2728901fd798 110 servoHL = yPos;
clementb 3:dd4efdfbbf01 111 wait(vitesse*10);
clementb 2:2728901fd798 112 }
clementb 2:2728901fd798 113 servoRL = 0;
clementb 2:2728901fd798 114 servoHL = 0;
clementb 2:2728901fd798 115 wait(1);
clementb 2:2728901fd798 116
clementb 3:dd4efdfbbf01 117
clementb 2:2728901fd798 118
clementb 2:2728901fd798 119 ledV = 1;
clementb 2:2728901fd798 120 ledY = 0;
clementb 2:2728901fd798 121 lcd.cls();
clementb 2:2728901fd798 122 lcd.locate(0,0);
clementb 3:dd4efdfbbf01 123 lcd.printf("Press a button..");
clementb 2:2728901fd798 124 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 125 lcd.printf("Currently : P%d ", page);
clementb 2:2728901fd798 126 wait(5);
clementb 2:2728901fd798 127
clementb 2:2728901fd798 128 if(menuR == 1) {
clementb 2:2728901fd798 129 page = page++;
clementb 2:2728901fd798 130 }
clementb 2:2728901fd798 131 else if(menuL == 1){
clementb 2:2728901fd798 132 page = page--;
clementb 2:2728901fd798 133 }
clementb 1:c3a33d9372f5 134
clementb 2:2728901fd798 135 if(page >5){
clementb 2:2728901fd798 136 page = 1;
clementb 2:2728901fd798 137 }
clementb 2:2728901fd798 138 else if(page<1){
clementb 2:2728901fd798 139 page = 5;
clementb 2:2728901fd798 140 }
clementb 2:2728901fd798 141
clementb 2:2728901fd798 142 ledV = 0;
clementb 2:2728901fd798 143 ledY = 1;
clementb 2:2728901fd798 144 lcd.locate(0, 1);
clementb 2:2728901fd798 145 lcd.printf("OK ! Next : P%d ", page);
clementb 2:2728901fd798 146 wait(3);
clementb 3:dd4efdfbbf01 147 lcd.cls();
clementb 3:dd4efdfbbf01 148 lcd.locate(0, 0);
clementb 3:dd4efdfbbf01 149 lcd.printf("Loading P%d...", page);
clementb 3:dd4efdfbbf01 150 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 151 lcd.printf("P%d :",page);
clementb 3:dd4efdfbbf01 152 lcd.locate(4, 1);
clementb 2:2728901fd798 153
clementb 2:2728901fd798 154 /*------------------------------------------- P4 : down > up --------------------------------------------------------------*/
clementb 1:c3a33d9372f5 155
clementb 1:c3a33d9372f5 156 if(page == 4) {
clementb 3:dd4efdfbbf01 157 lcd.printf("down > up ");
clementb 2:2728901fd798 158 servoHL = 0;
clementb 2:2728901fd798 159 i = 0;
clementb 2:2728901fd798 160 wait(3);
clementb 2:2728901fd798 161 lcd.locate(3, 1);
clementb 3:dd4efdfbbf01 162 lcd.printf(": x servo ");
clementb 3:dd4efdfbbf01 163 while(xPos<50){
clementb 2:2728901fd798 164 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 165 xPos = xPos+0.01;
clementb 3:dd4efdfbbf01 166 disp = xPos*100;
clementb 3:dd4efdfbbf01 167 servoHL = xPos;
clementb 3:dd4efdfbbf01 168 lcd.printf("%3d", disp);
clementb 2:2728901fd798 169 i = i++;
clementb 2:2728901fd798 170 wait(vitesse);
clementb 2:2728901fd798 171 }
clementb 2:2728901fd798 172 wait(1);
clementb 3:dd4efdfbbf01 173
clementb 3:dd4efdfbbf01 174
clementb 3:dd4efdfbbf01 175
clementb 2:2728901fd798 176 ledY = 0;
clementb 2:2728901fd798 177 lcd.cls();
clementb 1:c3a33d9372f5 178 lcd.locate(0,0);
clementb 1:c3a33d9372f5 179 lcd.printf("Running P4... ");
clementb 2:2728901fd798 180 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 181 lcd.printf(" 0 : y servo");
clementb 2:2728901fd798 182 ledV = 1;
clementb 3:dd4efdfbbf01 183 wait(1);
clementb 2:2728901fd798 184 lcd.locate(0,1);
clementb 3:dd4efdfbbf01 185 while(yPos<100){
clementb 2:2728901fd798 186 lcd.locate(0,1);
clementb 3:dd4efdfbbf01 187 yPos = yPos + 0.01;
clementb 3:dd4efdfbbf01 188 disp = yPos*100;
clementb 3:dd4efdfbbf01 189 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 190 servoHL = yPos;
clementb 2:2728901fd798 191 wait(vitesse);
clementb 2:2728901fd798 192 }
clementb 2:2728901fd798 193 wait(3);
clementb 2:2728901fd798 194 ledV = 0;
clementb 1:c3a33d9372f5 195 }
clementb 2:2728901fd798 196
clementb 2:2728901fd798 197 /*---------------------------------------------------- P3 : righ > left -----------------------------------------------------------------*/
clementb 2:2728901fd798 198
clementb 1:c3a33d9372f5 199 else if(page == 3) {
clementb 3:dd4efdfbbf01 200 lcd.printf("right > left");
clementb 2:2728901fd798 201 wait(3);
clementb 2:2728901fd798 202 lcd.locate(3, 1);
clementb 2:2728901fd798 203 lcd.printf(": y servo ");
clementb 3:dd4efdfbbf01 204 while(yPos<1.00){
clementb 3:dd4efdfbbf01 205 yPos = yPos+0.01;
clementb 2:2728901fd798 206 servoHL = yPos;
clementb 3:dd4efdfbbf01 207 disp = yPos*100;
clementb 3:dd4efdfbbf01 208 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 209 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 210 wait(vitesse*10);
clementb 2:2728901fd798 211 }
clementb 2:2728901fd798 212 i = 0;
clementb 2:2728901fd798 213 wait(1);
clementb 2:2728901fd798 214 lcd.locate(5, 1);
clementb 2:2728901fd798 215 lcd.printf("x");
clementb 3:dd4efdfbbf01 216 while(xPos<1.00){
clementb 3:dd4efdfbbf01 217 xPos = xPos+0.01;
clementb 2:2728901fd798 218 servoRL = xPos;
clementb 3:dd4efdfbbf01 219 disp = xPos*100;
clementb 3:dd4efdfbbf01 220 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 221 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 222 wait(vitesse*10);
clementb 2:2728901fd798 223 }
clementb 2:2728901fd798 224 i = 100;
clementb 2:2728901fd798 225 wait(1);
clementb 2:2728901fd798 226 lcd.locate(5, 1);
clementb 2:2728901fd798 227 lcd.printf("y");
clementb 3:dd4efdfbbf01 228 while(yPos>0.00){
clementb 3:dd4efdfbbf01 229 yPos = yPos-0.01;
clementb 2:2728901fd798 230 servoHL = yPos;
clementb 3:dd4efdfbbf01 231 disp = yPos*100;
clementb 3:dd4efdfbbf01 232 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 233 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 234 wait(vitesse*10);
clementb 2:2728901fd798 235 }
clementb 2:2728901fd798 236 wait(1);
clementb 2:2728901fd798 237 ledY = 0;
clementb 2:2728901fd798 238 lcd.cls();
clementb 1:c3a33d9372f5 239 lcd.locate(0,0);
clementb 1:c3a33d9372f5 240 lcd.printf("Running P3... ");
clementb 2:2728901fd798 241 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 242 lcd.printf("100 : x servo");
clementb 2:2728901fd798 243 ledV = 1;
clementb 2:2728901fd798 244 wait(2);
clementb 2:2728901fd798 245 i = 100;
clementb 3:dd4efdfbbf01 246 while(xPos>0){
clementb 3:dd4efdfbbf01 247 xPos = xPos-0.01;
clementb 3:dd4efdfbbf01 248 disp = xPos*100;
clementb 3:dd4efdfbbf01 249 servoRL = xPos;
clementb 2:2728901fd798 250 lcd.locate(0,1);
clementb 3:dd4efdfbbf01 251 lcd.printf("%3d",disp);
clementb 2:2728901fd798 252 wait(vitesse);
clementb 2:2728901fd798 253 }
clementb 2:2728901fd798 254 wait(3);
clementb 2:2728901fd798 255 ledV = 0;
clementb 1:c3a33d9372f5 256 }
clementb 2:2728901fd798 257
clementb 2:2728901fd798 258 /*---------------------------------------------------- P2 : up > down ----------------------------------------------------------------*/
clementb 2:2728901fd798 259
clementb 3:dd4efdfbbf01 260 else if(page == 2) {
clementb 3:dd4efdfbbf01 261 lcd.printf("up > down ");
clementb 2:2728901fd798 262 wait(3);
clementb 2:2728901fd798 263
clementb 2:2728901fd798 264
clementb 2:2728901fd798 265
clementb 2:2728901fd798 266 i = 0; //loading
clementb 2:2728901fd798 267 lcd.locate(3, 1);
clementb 2:2728901fd798 268 lcd.printf(": y servo ");
clementb 3:dd4efdfbbf01 269 while(yPos<1.00){
clementb 3:dd4efdfbbf01 270 yPos = yPos+0.01;
clementb 2:2728901fd798 271 servoHL = yPos;
clementb 3:dd4efdfbbf01 272 disp = yPos*100;
clementb 3:dd4efdfbbf01 273 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 274 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 275 wait(vitesse*10);
clementb 2:2728901fd798 276 }
clementb 2:2728901fd798 277 wait(1);
clementb 2:2728901fd798 278 lcd.locate(5, 1);
clementb 2:2728901fd798 279 lcd.printf("x");
clementb 3:dd4efdfbbf01 280 while(xPos<0.50){
clementb 3:dd4efdfbbf01 281 xPos = xPos+0.01;
clementb 2:2728901fd798 282 servoRL = xPos;
clementb 3:dd4efdfbbf01 283 disp = xPos*100;
clementb 3:dd4efdfbbf01 284 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 285 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 286 wait(vitesse*10);
clementb 2:2728901fd798 287 }
clementb 2:2728901fd798 288 wait(1);
clementb 2:2728901fd798 289 lcd.locate(5, 1);
clementb 2:2728901fd798 290 lcd.printf("y");
clementb 3:dd4efdfbbf01 291 while(yPos >0.61){
clementb 3:dd4efdfbbf01 292 yPos = yPos-0.01;
clementb 2:2728901fd798 293 servoHL = yPos;
clementb 3:dd4efdfbbf01 294 disp = yPos*100;
clementb 3:dd4efdfbbf01 295 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 296 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 297 wait(vitesse*10);
clementb 2:2728901fd798 298 }
clementb 2:2728901fd798 299 wait(1);
clementb 2:2728901fd798 300
clementb 2:2728901fd798 301
clementb 2:2728901fd798 302
clementb 2:2728901fd798 303 ledY = 0; //running
clementb 2:2728901fd798 304 lcd.cls();
clementb 1:c3a33d9372f5 305 lcd.locate(0,0);
clementb 1:c3a33d9372f5 306 lcd.printf("Running P2... ");
clementb 2:2728901fd798 307 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 308 lcd.printf(" 60: y degrees");
clementb 2:2728901fd798 309 ledV = 1;
clementb 3:dd4efdfbbf01 310 wait(1);
clementb 3:dd4efdfbbf01 311 yPos = 0.60;
clementb 3:dd4efdfbbf01 312 while(yPos>0.00){
clementb 3:dd4efdfbbf01 313 disp = yPos*100;
clementb 2:2728901fd798 314 lcd.locate(0,1);
clementb 3:dd4efdfbbf01 315 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 316 yPos = yPos-0.01;
clementb 3:dd4efdfbbf01 317 servoHL = yPos;
clementb 2:2728901fd798 318 wait(vitesse);
clementb 2:2728901fd798 319 }
clementb 3:dd4efdfbbf01 320 yPos = 0.00;
clementb 2:2728901fd798 321 wait(3);
clementb 2:2728901fd798 322 ledV = 0;
clementb 1:c3a33d9372f5 323 }
clementb 2:2728901fd798 324
clementb 2:2728901fd798 325 /*---------------------------------------------- P1 : left > righ ------------------------------------------------------------------*/
clementb 2:2728901fd798 326
clementb 3:dd4efdfbbf01 327 else if(page == 1){
clementb 3:dd4efdfbbf01 328 lcd.printf("left > right");
clementb 1:c3a33d9372f5 329 wait(2);
clementb 2:2728901fd798 330
clementb 2:2728901fd798 331
clementb 2:2728901fd798 332
clementb 2:2728901fd798 333 ledY = 0; //running
clementb 2:2728901fd798 334 lcd.cls();
clementb 1:c3a33d9372f5 335 lcd.locate(0,0);
clementb 1:c3a33d9372f5 336 lcd.printf("Running P1... ");
clementb 2:2728901fd798 337 lcd.locate(0, 1);
clementb 2:2728901fd798 338 lcd.printf(" 0 : x degrees");
clementb 2:2728901fd798 339 ledV = 1;
clementb 2:2728901fd798 340 wait(2);
clementb 3:dd4efdfbbf01 341 xPos = 0.00;
clementb 3:dd4efdfbbf01 342 while(xPos<1.00){
clementb 3:dd4efdfbbf01 343 xPos = xPos+0.01;
clementb 3:dd4efdfbbf01 344 servoRL = xPos;
clementb 3:dd4efdfbbf01 345 disp = xPos*100;
clementb 2:2728901fd798 346 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 347 lcd.printf("%3d", disp);
clementb 2:2728901fd798 348 wait(vitesse);
clementb 2:2728901fd798 349 }
clementb 2:2728901fd798 350 wait(3);
clementb 2:2728901fd798 351 ledV = 0;
clementb 1:c3a33d9372f5 352 }
clementb 2:2728901fd798 353
clementb 2:2728901fd798 354 /*------------------------------------------------ P5 : manual mode ----------------------------------------------------------------*/
clementb 2:2728901fd798 355
clementb 2:2728901fd798 356 if(page == 5){ //home screen
clementb 2:2728901fd798 357 ledY = 1;
clementb 2:2728901fd798 358 lcd.cls();
clementb 2:2728901fd798 359 lcd.locate(0, 0);
clementb 2:2728901fd798 360 lcd.printf("P5 : manual mode");
clementb 2:2728901fd798 361 lcd.locate(0,1);
clementb 2:2728901fd798 362 lcd.printf("Button : exit");
clementb 2:2728901fd798 363 wait(3);
clementb 2:2728901fd798 364
clementb 2:2728901fd798 365
clementb 2:2728901fd798 366
clementb 2:2728901fd798 367 lcd.cls(); //loading
clementb 2:2728901fd798 368 lcd.locate(0, 0);
clementb 2:2728901fd798 369 lcd.printf("Servomotors will");
clementb 2:2728901fd798 370 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 371 lcd.printf("be positioned as");
clementb 2:2728901fd798 372 wait(3);
clementb 2:2728901fd798 373 lcd.locate(0, 0);
clementb 3:dd4efdfbbf01 374 lcd.printf("be positioned as");
clementb 2:2728901fd798 375 lcd.locate(0, 1);
clementb 2:2728901fd798 376 lcd.printf("potentiometers, ");
clementb 2:2728901fd798 377 wait(3);
clementb 2:2728901fd798 378 lcd.locate(0, 0);
clementb 2:2728901fd798 379 lcd.printf("potentiometers, ");
clementb 2:2728901fd798 380 lcd.locate(0, 1);
clementb 2:2728901fd798 381 lcd.printf("please wait. ");
clementb 2:2728901fd798 382 wait(3);
clementb 2:2728901fd798 383 lcd.cls();
clementb 2:2728901fd798 384 lcd.locate(4, 0);
clementb 2:2728901fd798 385 lcd.printf(": x servo");
clementb 2:2728901fd798 386 lcd.locate(4, 1);
clementb 2:2728901fd798 387 lcd.printf(": y servo");
clementb 2:2728901fd798 388 float mesure = analogX.read();
clementb 3:dd4efdfbbf01 389 xPos = (int) mesure*100;
clementb 3:dd4efdfbbf01 390 xPos = xPos/100;
clementb 3:dd4efdfbbf01 391 if(xPos>mesure){
clementb 3:dd4efdfbbf01 392 while(xPos>mesure){
clementb 3:dd4efdfbbf01 393 xPos = xPos-0.01;
clementb 3:dd4efdfbbf01 394 servoRL = xPos;
clementb 3:dd4efdfbbf01 395 disp = xPos*100;
clementb 3:dd4efdfbbf01 396 lcd.locate(0, 0);
clementb 3:dd4efdfbbf01 397 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 398 wait(vitesse*10);
clementb 3:dd4efdfbbf01 399 }
clementb 3:dd4efdfbbf01 400 }
clementb 3:dd4efdfbbf01 401 if(xPos<mesure){
clementb 3:dd4efdfbbf01 402 while(xPos<mesure){
clementb 3:dd4efdfbbf01 403 xPos = xPos+0.01;
clementb 3:dd4efdfbbf01 404 servoRL = xPos;
clementb 3:dd4efdfbbf01 405 disp = xPos*100;
clementb 3:dd4efdfbbf01 406 lcd.locate(0, 0);
clementb 3:dd4efdfbbf01 407 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 408 wait(vitesse*10);
clementb 3:dd4efdfbbf01 409 }
clementb 2:2728901fd798 410 }
clementb 3:dd4efdfbbf01 411
clementb 2:2728901fd798 412 mesure = analogY.read();
clementb 3:dd4efdfbbf01 413 yPos = (int) mesure*100;
clementb 3:dd4efdfbbf01 414 yPos = yPos/100;
clementb 3:dd4efdfbbf01 415 while(yPos!=mesure){
clementb 3:dd4efdfbbf01 416 if(yPos>mesure){
clementb 3:dd4efdfbbf01 417 yPos = yPos-0.01;
clementb 3:dd4efdfbbf01 418 servoHL = yPos;
clementb 3:dd4efdfbbf01 419 disp = yPos*100;
clementb 3:dd4efdfbbf01 420 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 421 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 422 wait(vitesse*10);
clementb 3:dd4efdfbbf01 423 }
clementb 3:dd4efdfbbf01 424 else if(yPos<mesure){
clementb 3:dd4efdfbbf01 425 yPos = yPos+0.01;
clementb 3:dd4efdfbbf01 426 servoHL = yPos;
clementb 3:dd4efdfbbf01 427 disp = yPos*100;
clementb 3:dd4efdfbbf01 428 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 429 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 430 wait(vitesse);
clementb 3:dd4efdfbbf01 431 }
clementb 2:2728901fd798 432 }
clementb 2:2728901fd798 433 wait(1);
clementb 2:2728901fd798 434
clementb 2:2728901fd798 435
clementb 2:2728901fd798 436
clementb 2:2728901fd798 437 ledY = 0; //running
clementb 2:2728901fd798 438 ledV = 1;
clementb 2:2728901fd798 439 while(menuR == 0 && menuL == 0){
clementb 3:dd4efdfbbf01 440 servoRL = analogX.read();
clementb 3:dd4efdfbbf01 441 xPos = analogX.read();
clementb 3:dd4efdfbbf01 442 disp = xPos*100;
clementb 3:dd4efdfbbf01 443 lcd.locate(0, 0);
clementb 3:dd4efdfbbf01 444 lcd.printf("%3d", disp);
clementb 3:dd4efdfbbf01 445 servoHL = analogY.read();
clementb 3:dd4efdfbbf01 446 yPos = analogY.read();
clementb 3:dd4efdfbbf01 447 yPos = analogY.read();
clementb 3:dd4efdfbbf01 448 disp = yPos*100;
clementb 3:dd4efdfbbf01 449 lcd.locate(0, 1);
clementb 3:dd4efdfbbf01 450 lcd.printf("%3d", disp);
clementb 2:2728901fd798 451 wait(vitesse);
clementb 3:dd4efdfbbf01 452 }
clementb 2:2728901fd798 453 ledV = 0;
clementb 1:c3a33d9372f5 454 }
clementb 2:2728901fd798 455 }
clementb 0:9438fe754ab6 456 }