Clément Ballot / Mbed 2 deprecated BIXI_arm_2_axes

Dependencies:   Servo TextLCD mbed

main.cpp@2:2728901fd798, 2016-06-16 (annotated)

Committer:
clementb
Date:
Thu Jun 16 13:00:47 2016 +0000
Revision:
2:2728901fd798
Parent:
1:c3a33d9372f5
Child:
3:dd4efdfbbf01
This version is tested without servomotors, so I think it work... But all is OK.

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 2:2728901fd798 36 int vitesse = 0.05; //------------------a------------- CHANGE IT TO REDUCE/INCREASE SERVOMOTORS SPEED ----------------------------
clementb 0:9438fe754ab6 37 int page;
clementb 2:2728901fd798 38 int i = 0;
clementb 0:9438fe754ab6 39 int xPos = 0;
clementb 0:9438fe754ab6 40 int yPos = 0;
clementb 0:9438fe754ab6 41
clementb 1:c3a33d9372f5 42
clementb 1:c3a33d9372f5 43 int main(){
clementb 1:c3a33d9372f5 44 page = 1;
clementb 0:9438fe754ab6 45 lcd.cls();
clementb 0:9438fe754ab6 46 lcd.locate(0,0);
clementb 2:2728901fd798 47 lcd.printf("BIXI Rob'Arm :by");
clementb 1:c3a33d9372f5 48 lcd.locate(0,1);
clementb 2:2728901fd798 49 lcd.printf("Clement BALLOT");
clementb 1:c3a33d9372f5 50 servoRL = 0;
clementb 1:c3a33d9372f5 51 servoHL = 0;
clementb 2:2728901fd798 52 while(i != 50){
clementb 2:2728901fd798 53 ledY = 1;
clementb 2:2728901fd798 54 ledV = 0;
clementb 1:c3a33d9372f5 55 wait(0.05);
clementb 2:2728901fd798 56 ledY = 0;
clementb 2:2728901fd798 57 ledV = 1;
clementb 1:c3a33d9372f5 58 wait(0.05);
clementb 2:2728901fd798 59 i = i++;
clementb 0:9438fe754ab6 60 }
clementb 2:2728901fd798 61 ledY = 0;
clementb 2:2728901fd798 62 ledV = 0;
clementb 0:9438fe754ab6 63
clementb 1:c3a33d9372f5 64 while(1) {
clementb 2:2728901fd798 65 ledY = 1;
clementb 2:2728901fd798 66 lcd.cls();
clementb 2:2728901fd798 67 lcd.locate(0,0);
clementb 2:2728901fd798 68 lcd.printf("Reset servo pos");
clementb 2:2728901fd798 69 wait(2);
clementb 2:2728901fd798 70 lcd.locate(0, 1);
clementb 2:2728901fd798 71 lcd.printf("Please wait...");
clementb 2:2728901fd798 72 wait(3);
clementb 2:2728901fd798 73 lcd.cls();
clementb 2:2728901fd798 74 lcd.locate(3, 0);
clementb 2:2728901fd798 75 lcd.printf(" : x servo");
clementb 2:2728901fd798 76 lcd.locate(3, 1);
clementb 2:2728901fd798 77 lcd.printf(" : y servo");
clementb 2:2728901fd798 78 lcd.locate(0,0);
clementb 2:2728901fd798 79 lcd.printf("%3d", xPos);
clementb 2:2728901fd798 80 lcd.locate(0, 1);
clementb 2:2728901fd798 81 lcd.printf("%3d", yPos);
clementb 2:2728901fd798 82 if(xPos !=0){
clementb 2:2728901fd798 83 while(yPos != 120){
clementb 2:2728901fd798 84 yPos = yPos + 1;
clementb 2:2728901fd798 85 lcd.locate(0, 1);
clementb 2:2728901fd798 86 lcd.printf("%3d", yPos);
clementb 2:2728901fd798 87 servoHL = yPos;
clementb 2:2728901fd798 88 wait(vitesse);
clementb 1:c3a33d9372f5 89 }
clementb 0:9438fe754ab6 90 }
clementb 2:2728901fd798 91 while(xPos != 0){
clementb 2:2728901fd798 92 xPos = xPos - 0.01;
clementb 2:2728901fd798 93 lcd.locate(0,0);
clementb 2:2728901fd798 94 lcd.printf("%3d", xPos);
clementb 2:2728901fd798 95 servoRL = xPos;
clementb 2:2728901fd798 96 wait(vitesse);
clementb 2:2728901fd798 97 }
clementb 2:2728901fd798 98 while(yPos !=0){
clementb 2:2728901fd798 99 yPos = yPos - 1;
clementb 2:2728901fd798 100 lcd.locate(0, 1);
clementb 2:2728901fd798 101 lcd.printf("%3d", yPos);
clementb 2:2728901fd798 102 servoHL = yPos;
clementb 2:2728901fd798 103 wait(vitesse);
clementb 2:2728901fd798 104 }
clementb 2:2728901fd798 105 servoRL = 0;
clementb 2:2728901fd798 106 servoHL = 0;
clementb 2:2728901fd798 107 wait(1);
clementb 2:2728901fd798 108
clementb 2:2728901fd798 109 lcd.cls();
clementb 2:2728901fd798 110 lcd.locate(0,0);
clementb 2:2728901fd798 111 lcd.printf("The program will");
clementb 2:2728901fd798 112 lcd.locate(0, 1);
clementb 2:2728901fd798 113 lcd.printf("read the button");
clementb 2:2728901fd798 114 wait(2);
clementb 2:2728901fd798 115 lcd.cls();
clementb 2:2728901fd798 116 lcd.locate(0,0);
clementb 2:2728901fd798 117 lcd.printf("read the button");
clementb 2:2728901fd798 118 lcd.locate(0, 1);
clementb 2:2728901fd798 119 lcd.printf("states. When");
clementb 2:2728901fd798 120 wait(2);
clementb 2:2728901fd798 121 lcd.cls();
clementb 2:2728901fd798 122 lcd.locate(0,0);
clementb 2:2728901fd798 123 lcd.printf("states. When");
clementb 2:2728901fd798 124 lcd.locate(0, 1);
clementb 2:2728901fd798 125 lcd.printf("the light turn");
clementb 2:2728901fd798 126 wait(2);
clementb 2:2728901fd798 127 lcd.cls();
clementb 2:2728901fd798 128 lcd.locate(0,0);
clementb 2:2728901fd798 129 lcd.printf("the light turn");
clementb 2:2728901fd798 130 lcd.locate(0, 1);
clementb 2:2728901fd798 131 lcd.printf("green, press a");
clementb 2:2728901fd798 132 wait(2);
clementb 2:2728901fd798 133 lcd.cls();
clementb 2:2728901fd798 134 lcd.locate(0, 0);
clementb 2:2728901fd798 135 lcd.printf("green, press a");
clementb 2:2728901fd798 136 lcd.locate(0, 1);
clementb 2:2728901fd798 137 lcd.printf("button to change");
clementb 2:2728901fd798 138 wait(2);
clementb 2:2728901fd798 139 lcd.cls();
clementb 2:2728901fd798 140 lcd.locate(0, 0);
clementb 2:2728901fd798 141 lcd.printf("button to change");
clementb 2:2728901fd798 142 lcd.locate(0, 1);
clementb 2:2728901fd798 143 lcd.printf("the programm,");
clementb 2:2728901fd798 144 wait(2);
clementb 2:2728901fd798 145 lcd.cls();
clementb 2:2728901fd798 146 lcd.locate(0, 0);
clementb 2:2728901fd798 147 lcd.printf("the programm,");
clementb 2:2728901fd798 148 lcd.locate(0, 1);
clementb 2:2728901fd798 149 lcd.printf("from 1 to 5.");
clementb 2:2728901fd798 150 wait(2);
clementb 2:2728901fd798 151
clementb 2:2728901fd798 152 ledV = 1;
clementb 2:2728901fd798 153 ledY = 0;
clementb 2:2728901fd798 154 lcd.cls();
clementb 2:2728901fd798 155 lcd.locate(0,0);
clementb 2:2728901fd798 156 lcd.printf("Reading states..");
clementb 2:2728901fd798 157 lcd.locate(0, 1);
clementb 2:2728901fd798 158 lcd.printf("Currentrly : P%d ", page);
clementb 2:2728901fd798 159 wait(5);
clementb 2:2728901fd798 160
clementb 2:2728901fd798 161 if(menuR == 1) {
clementb 2:2728901fd798 162 page = page++;
clementb 2:2728901fd798 163 }
clementb 2:2728901fd798 164 else if(menuL == 1){
clementb 2:2728901fd798 165 page = page--;
clementb 2:2728901fd798 166 }
clementb 1:c3a33d9372f5 167
clementb 2:2728901fd798 168 if(page >5){
clementb 2:2728901fd798 169 page = 1;
clementb 2:2728901fd798 170 }
clementb 2:2728901fd798 171 else if(page<1){
clementb 2:2728901fd798 172 page = 5;
clementb 2:2728901fd798 173 }
clementb 2:2728901fd798 174
clementb 2:2728901fd798 175 ledV = 0;
clementb 2:2728901fd798 176 ledY = 1;
clementb 2:2728901fd798 177 lcd.locate(0, 1);
clementb 2:2728901fd798 178 lcd.printf("OK ! Next : P%d ", page);
clementb 2:2728901fd798 179 wait(3);
clementb 2:2728901fd798 180
clementb 2:2728901fd798 181 /*------------------------------------------- P4 : down > up --------------------------------------------------------------*/
clementb 1:c3a33d9372f5 182
clementb 1:c3a33d9372f5 183 if(page == 4) {
clementb 2:2728901fd798 184 ledY = 1;
clementb 1:c3a33d9372f5 185 lcd.cls();
clementb 1:c3a33d9372f5 186 lcd.locate(0,0);
clementb 1:c3a33d9372f5 187 lcd.printf("Loading P4... ");
clementb 1:c3a33d9372f5 188 lcd.locate(0,1);
clementb 2:2728901fd798 189 lcd.printf("P4 : down > up ");
clementb 2:2728901fd798 190 servoHL = 0;
clementb 2:2728901fd798 191 i = 0;
clementb 2:2728901fd798 192 wait(3);
clementb 2:2728901fd798 193 lcd.locate(3, 1);
clementb 2:2728901fd798 194 lcd.printf(": y servo ");
clementb 2:2728901fd798 195 while(i !=50){
clementb 2:2728901fd798 196 lcd.locate(0, 1);
clementb 2:2728901fd798 197 xPos = xPos++;
clementb 2:2728901fd798 198 servoRL = xPos;
clementb 2:2728901fd798 199 lcd.printf("%3d", xPos);
clementb 2:2728901fd798 200 i = i++;
clementb 2:2728901fd798 201 wait(vitesse);
clementb 2:2728901fd798 202 }
clementb 2:2728901fd798 203 wait(1);
clementb 2:2728901fd798 204 ledY = 0;
clementb 2:2728901fd798 205 lcd.cls();
clementb 1:c3a33d9372f5 206 lcd.locate(0,0);
clementb 1:c3a33d9372f5 207 lcd.printf("Running P4... ");
clementb 2:2728901fd798 208 lcd.locate(0, 1);
clementb 2:2728901fd798 209 lcd.printf(" 0 degrees");
clementb 2:2728901fd798 210 i = 0;
clementb 2:2728901fd798 211 ledV = 1;
clementb 2:2728901fd798 212 wait(2);
clementb 2:2728901fd798 213 lcd.locate(0,1);
clementb 2:2728901fd798 214 while(i!=100){
clementb 2:2728901fd798 215 i = i+1;
clementb 2:2728901fd798 216 lcd.locate(0,1);
clementb 2:2728901fd798 217 lcd.printf("%3d",i);
clementb 2:2728901fd798 218 servoHL = i;
clementb 2:2728901fd798 219 wait(vitesse);
clementb 2:2728901fd798 220 }
clementb 2:2728901fd798 221 yPos = 100;
clementb 2:2728901fd798 222 wait(3);
clementb 2:2728901fd798 223 ledV = 0;
clementb 1:c3a33d9372f5 224 }
clementb 2:2728901fd798 225
clementb 2:2728901fd798 226 /*---------------------------------------------------- P3 : righ > left -----------------------------------------------------------------*/
clementb 2:2728901fd798 227
clementb 1:c3a33d9372f5 228 else if(page == 3) {
clementb 2:2728901fd798 229 ledY = 1;
clementb 1:c3a33d9372f5 230 lcd.cls();
clementb 1:c3a33d9372f5 231 lcd.locate(0,0);
clementb 1:c3a33d9372f5 232 lcd.printf("Loading P3... ");
clementb 1:c3a33d9372f5 233 lcd.locate(0,1);
clementb 2:2728901fd798 234 lcd.printf("P3: right > left");
clementb 2:2728901fd798 235 i = 0;
clementb 2:2728901fd798 236 wait(3);
clementb 2:2728901fd798 237 lcd.locate(3, 1);
clementb 2:2728901fd798 238 lcd.printf(": y servo ");
clementb 2:2728901fd798 239 while(i !=120){
clementb 2:2728901fd798 240 lcd.locate(0, 1);
clementb 2:2728901fd798 241 yPos = yPos++;
clementb 2:2728901fd798 242 servoHL = yPos;
clementb 2:2728901fd798 243 lcd.printf("%3d", yPos);
clementb 2:2728901fd798 244 i = i++;
clementb 2:2728901fd798 245 wait(vitesse);
clementb 2:2728901fd798 246 }
clementb 2:2728901fd798 247 i = 0;
clementb 2:2728901fd798 248 wait(1);
clementb 2:2728901fd798 249 lcd.locate(5, 1);
clementb 2:2728901fd798 250 lcd.printf("x");
clementb 2:2728901fd798 251 while(i !=100){
clementb 2:2728901fd798 252 lcd.locate(0, 1);
clementb 2:2728901fd798 253 xPos = xPos++;
clementb 2:2728901fd798 254 servoRL = xPos;
clementb 2:2728901fd798 255 lcd.printf("%3d", xPos);
clementb 2:2728901fd798 256 i = i++;
clementb 2:2728901fd798 257 wait(vitesse);
clementb 2:2728901fd798 258 }
clementb 2:2728901fd798 259 i = 100;
clementb 2:2728901fd798 260 wait(1);
clementb 2:2728901fd798 261 lcd.locate(5, 1);
clementb 2:2728901fd798 262 lcd.printf("y");
clementb 2:2728901fd798 263 while(i != 0){
clementb 2:2728901fd798 264 lcd.locate(0, 1);
clementb 2:2728901fd798 265 yPos = yPos--;
clementb 2:2728901fd798 266 servoHL = yPos;
clementb 2:2728901fd798 267 lcd.printf("%3d", yPos);
clementb 2:2728901fd798 268 i = i--;
clementb 2:2728901fd798 269 wait(vitesse);
clementb 2:2728901fd798 270 }
clementb 2:2728901fd798 271 wait(1);
clementb 2:2728901fd798 272 ledY = 0;
clementb 2:2728901fd798 273 lcd.cls();
clementb 1:c3a33d9372f5 274 lcd.locate(0,0);
clementb 1:c3a33d9372f5 275 lcd.printf("Running P3... ");
clementb 2:2728901fd798 276 lcd.locate(0, 1);
clementb 2:2728901fd798 277 lcd.printf("100 degrees");
clementb 2:2728901fd798 278 ledV = 1;
clementb 2:2728901fd798 279 wait(2);
clementb 2:2728901fd798 280 i = 100;
clementb 2:2728901fd798 281 while(i!=0){
clementb 2:2728901fd798 282 i = i-1;
clementb 2:2728901fd798 283 lcd.locate(0,1);
clementb 2:2728901fd798 284 lcd.printf("%3d",i);
clementb 2:2728901fd798 285 servoRL = i;
clementb 2:2728901fd798 286 wait(vitesse);
clementb 2:2728901fd798 287 }
clementb 2:2728901fd798 288 xPos = 0;
clementb 2:2728901fd798 289 wait(3);
clementb 2:2728901fd798 290 ledV = 0;
clementb 1:c3a33d9372f5 291 }
clementb 2:2728901fd798 292
clementb 2:2728901fd798 293 /*---------------------------------------------------- P2 : up > down ----------------------------------------------------------------*/
clementb 2:2728901fd798 294
clementb 2:2728901fd798 295 else if(page == 2) { //home screen
clementb 2:2728901fd798 296 ledY = 1;
clementb 1:c3a33d9372f5 297 lcd.cls();
clementb 1:c3a33d9372f5 298 lcd.locate(0,0);
clementb 1:c3a33d9372f5 299 lcd.printf("Loading P2...");
clementb 1:c3a33d9372f5 300 lcd.locate(0,1);
clementb 2:2728901fd798 301 lcd.printf("P2 : up > down ");
clementb 2:2728901fd798 302 wait(3);
clementb 2:2728901fd798 303
clementb 2:2728901fd798 304
clementb 2:2728901fd798 305
clementb 2:2728901fd798 306 i = 0; //loading
clementb 2:2728901fd798 307 lcd.locate(3, 1);
clementb 2:2728901fd798 308 lcd.printf(": y servo ");
clementb 2:2728901fd798 309 while(i != 120){
clementb 2:2728901fd798 310 lcd.locate(0, 1);
clementb 2:2728901fd798 311 yPos = yPos++;
clementb 2:2728901fd798 312 servoHL = yPos;
clementb 2:2728901fd798 313 lcd.printf("%3d", yPos);
clementb 2:2728901fd798 314 i = i++;
clementb 2:2728901fd798 315 wait(vitesse);
clementb 2:2728901fd798 316 }
clementb 2:2728901fd798 317 i = 0;
clementb 2:2728901fd798 318 wait(1);
clementb 2:2728901fd798 319 lcd.locate(5, 1);
clementb 2:2728901fd798 320 lcd.printf("x");
clementb 2:2728901fd798 321 while(i != 50){
clementb 2:2728901fd798 322 lcd.locate(0, 1);
clementb 2:2728901fd798 323 xPos = xPos++;
clementb 2:2728901fd798 324 servoRL = xPos;
clementb 2:2728901fd798 325 lcd.printf("%3d", xPos);
clementb 2:2728901fd798 326 i = i++;
clementb 2:2728901fd798 327 wait(vitesse);
clementb 2:2728901fd798 328 }
clementb 2:2728901fd798 329 i = 120;
clementb 2:2728901fd798 330 wait(1);
clementb 2:2728901fd798 331 lcd.locate(5, 1);
clementb 2:2728901fd798 332 lcd.printf("y");
clementb 2:2728901fd798 333 while(i!= 100){
clementb 2:2728901fd798 334 lcd.locate(0, 1);
clementb 2:2728901fd798 335 yPos = yPos--;
clementb 2:2728901fd798 336 servoHL = yPos;
clementb 2:2728901fd798 337 lcd.printf("%3d", yPos);
clementb 2:2728901fd798 338 i = i--;
clementb 2:2728901fd798 339 wait(vitesse);
clementb 2:2728901fd798 340 }
clementb 2:2728901fd798 341 wait(1);
clementb 2:2728901fd798 342
clementb 2:2728901fd798 343
clementb 2:2728901fd798 344
clementb 2:2728901fd798 345 ledY = 0; //running
clementb 2:2728901fd798 346 lcd.cls();
clementb 1:c3a33d9372f5 347 lcd.locate(0,0);
clementb 1:c3a33d9372f5 348 lcd.printf("Running P2... ");
clementb 2:2728901fd798 349 lcd.locate(0, 1);
clementb 2:2728901fd798 350 lcd.printf("100 : y degrees");
clementb 2:2728901fd798 351 ledV = 1;
clementb 2:2728901fd798 352 wait(2);
clementb 2:2728901fd798 353 i = 100;
clementb 2:2728901fd798 354 while(i!=0){
clementb 2:2728901fd798 355 i = i-1;
clementb 2:2728901fd798 356 lcd.locate(0,1);
clementb 2:2728901fd798 357 lcd.printf("%3d", i);
clementb 2:2728901fd798 358 servoHL = i;
clementb 2:2728901fd798 359 wait(vitesse);
clementb 2:2728901fd798 360 }
clementb 2:2728901fd798 361 yPos = 0;
clementb 2:2728901fd798 362 wait(3);
clementb 2:2728901fd798 363 ledV = 0;
clementb 1:c3a33d9372f5 364 }
clementb 2:2728901fd798 365
clementb 2:2728901fd798 366 /*---------------------------------------------- P1 : left > righ ------------------------------------------------------------------*/
clementb 2:2728901fd798 367
clementb 2:2728901fd798 368 else if(page == 1) { //home screen
clementb 2:2728901fd798 369 ledY = 1;
clementb 1:c3a33d9372f5 370 lcd.cls();
clementb 1:c3a33d9372f5 371 lcd.locate(0,0);
clementb 1:c3a33d9372f5 372 lcd.printf("Loading P1... ");
clementb 1:c3a33d9372f5 373 lcd.locate(0,1);
clementb 2:2728901fd798 374 lcd.printf("P1: left > right");
clementb 1:c3a33d9372f5 375 wait(2);
clementb 2:2728901fd798 376
clementb 2:2728901fd798 377
clementb 2:2728901fd798 378
clementb 2:2728901fd798 379 ledY = 0; //running
clementb 2:2728901fd798 380 lcd.cls();
clementb 1:c3a33d9372f5 381 lcd.locate(0,0);
clementb 1:c3a33d9372f5 382 lcd.printf("Running P1... ");
clementb 2:2728901fd798 383 lcd.locate(0, 1);
clementb 2:2728901fd798 384 lcd.printf(" 0 : x degrees");
clementb 2:2728901fd798 385 ledV = 1;
clementb 2:2728901fd798 386 wait(2);
clementb 2:2728901fd798 387 i = 0;
clementb 2:2728901fd798 388 while(i!=100){
clementb 2:2728901fd798 389 i = i+1;
clementb 2:2728901fd798 390 lcd.locate(0, 1);
clementb 2:2728901fd798 391 lcd.printf("%3d", i);
clementb 2:2728901fd798 392 servoRL = i;
clementb 2:2728901fd798 393 wait(vitesse);
clementb 2:2728901fd798 394 }
clementb 2:2728901fd798 395 xPos = 100;
clementb 2:2728901fd798 396 wait(3);
clementb 2:2728901fd798 397 ledV = 0;
clementb 1:c3a33d9372f5 398 }
clementb 2:2728901fd798 399
clementb 2:2728901fd798 400 /*------------------------------------------------ P5 : manual mode ----------------------------------------------------------------*/
clementb 2:2728901fd798 401
clementb 2:2728901fd798 402 if(page == 5){ //home screen
clementb 2:2728901fd798 403 ledY = 1;
clementb 2:2728901fd798 404 lcd.cls();
clementb 2:2728901fd798 405 lcd.locate(0, 0);
clementb 2:2728901fd798 406 lcd.printf("P5 : manual mode");
clementb 2:2728901fd798 407 lcd.locate(0,1);
clementb 2:2728901fd798 408 lcd.printf("Button : exit");
clementb 2:2728901fd798 409 wait(3);
clementb 2:2728901fd798 410
clementb 2:2728901fd798 411
clementb 2:2728901fd798 412
clementb 2:2728901fd798 413 lcd.cls(); //loading
clementb 2:2728901fd798 414 lcd.locate(0, 0);
clementb 2:2728901fd798 415 lcd.printf("Servomotors will");
clementb 2:2728901fd798 416 lcd.locate(0, 1);
clementb 2:2728901fd798 417 lcd.printf("be as ");
clementb 2:2728901fd798 418 wait(3);
clementb 2:2728901fd798 419 lcd.locate(0, 0);
clementb 2:2728901fd798 420 lcd.printf("be as ");
clementb 2:2728901fd798 421 lcd.locate(0, 1);
clementb 2:2728901fd798 422 lcd.printf("potentiometers, ");
clementb 2:2728901fd798 423 wait(3);
clementb 2:2728901fd798 424 lcd.locate(0, 0);
clementb 2:2728901fd798 425 lcd.printf("potentiometers, ");
clementb 2:2728901fd798 426 lcd.locate(0, 1);
clementb 2:2728901fd798 427 lcd.printf("please wait. ");
clementb 2:2728901fd798 428 wait(3);
clementb 2:2728901fd798 429 lcd.cls();
clementb 2:2728901fd798 430 lcd.locate(4, 0);
clementb 2:2728901fd798 431 lcd.printf(": x servo");
clementb 2:2728901fd798 432 lcd.locate(4, 1);
clementb 2:2728901fd798 433 lcd.printf(": y servo");
clementb 2:2728901fd798 434 float mesure = analogX.read();
clementb 2:2728901fd798 435 mesure = mesure*100;
clementb 2:2728901fd798 436 mesure = 1.2*mesure;
clementb 2:2728901fd798 437 xPos = (int) mesure;
clementb 2:2728901fd798 438 int i =0;
clementb 2:2728901fd798 439 while(xPos != i){
clementb 2:2728901fd798 440 lcd.locate(0, 0);
clementb 2:2728901fd798 441 lcd.printf("%3d", i);
clementb 2:2728901fd798 442 i = i++;
clementb 2:2728901fd798 443 servoRL = i;
clementb 2:2728901fd798 444 wait(vitesse);
clementb 2:2728901fd798 445 }
clementb 2:2728901fd798 446 mesure = analogY.read();
clementb 2:2728901fd798 447 mesure = mesure*100;
clementb 2:2728901fd798 448 mesure = 1.2*mesure;
clementb 2:2728901fd798 449 yPos = (int) mesure;
clementb 2:2728901fd798 450 i =0;
clementb 2:2728901fd798 451 while(i != yPos){
clementb 2:2728901fd798 452 lcd.locate(0, 1);
clementb 2:2728901fd798 453 lcd.printf("%3d", i);
clementb 2:2728901fd798 454 i = i++;
clementb 2:2728901fd798 455 servoHL = i;
clementb 2:2728901fd798 456 wait(vitesse);
clementb 2:2728901fd798 457 }
clementb 2:2728901fd798 458 wait(1);
clementb 2:2728901fd798 459
clementb 2:2728901fd798 460
clementb 2:2728901fd798 461
clementb 2:2728901fd798 462 ledY = 0; //running
clementb 2:2728901fd798 463 ledV = 1;
clementb 2:2728901fd798 464 while(menuR == 0 && menuL == 0){
clementb 2:2728901fd798 465 float mesure = analogX.read();
clementb 2:2728901fd798 466 mesure = mesure*100;
clementb 2:2728901fd798 467 mesure = 1.2*mesure;
clementb 2:2728901fd798 468 xPos = (int) mesure;
clementb 2:2728901fd798 469 lcd.locate(0,0);
clementb 2:2728901fd798 470 lcd.printf("%3d", xPos);
clementb 2:2728901fd798 471 servoRL = xPos;
clementb 2:2728901fd798 472 mesure = analogY.read();
clementb 2:2728901fd798 473 mesure = mesure*100;
clementb 2:2728901fd798 474 mesure = 1.2*mesure;
clementb 2:2728901fd798 475 yPos = (int) mesure;
clementb 2:2728901fd798 476 lcd.locate(0,1);
clementb 2:2728901fd798 477 lcd.printf("%3d", yPos);
clementb 2:2728901fd798 478 servoHL = yPos;
clementb 2:2728901fd798 479 wait(vitesse);
clementb 2:2728901fd798 480 }
clementb 2:2728901fd798 481 ledV = 0;
clementb 1:c3a33d9372f5 482 }
clementb 2:2728901fd798 483 }
clementb 0:9438fe754ab6 484 }