Enrico Marinoni / Mbed 2 deprecated ScatolaMagica

BY: www.emcu.eu Dates: 08-12-2017 Magic Box - Ver. 1.0 Servo Motor: MG996R This example was tested on NUCLEO-F401RE If you use a terminal emulator (9600,8,1) on your PC do you also see the commands

Dependencies:   mbed

Fork of ScatolaMagica by Enrico Marinoni

main.cpp@1:e1acd469838f, 2017-12-10 (annotated)

Committer:
emcu
Date:
Sun Dec 10 02:31:15 2017 +0000
Revision:
1:e1acd469838f
Parent:
0:4192ed0f6a57 
Magic Box made using NUCLEO-F401RE and two RC motors MG996R

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emcu 0:4192ed0f6a57 1 /*
emcu 0:4192ed0f6a57 2
emcu 0:4192ed0f6a57 3 BY: www.emcu.eu
emcu 0:4192ed0f6a57 4 Date: 08-12-2017
emcu 0:4192ed0f6a57 5 Scatola Magica - Ver. 1.0
emcu 0:4192ed0f6a57 6 Servo Motor: MG996R
emcu 0:4192ed0f6a57 7 This example was tested on NUCLEO-F401RE
emcu 0:4192ed0f6a57 8 If you use a terminal emulator (9600,8,1) on your PC
emcu 0:4192ed0f6a57 9 you also see the commands sends do RC motor.
emcu 0:4192ed0f6a57 10
emcu 0:4192ed0f6a57 11
emcu 0:4192ed0f6a57 12 ---------------------------------------------------------------------------
emcu 0:4192ed0f6a57 13
emcu 0:4192ed0f6a57 14 Servo Motor:
emcu 0:4192ed0f6a57 15 MG996R - http://www.electronicoscaldas.com/datasheet/MG996R_Tower-Pro.pdf
emcu 0:4192ed0f6a57 16
emcu 0:4192ed0f6a57 17 RED - + Vcc from 4,5 to 7,2V
emcu 0:4192ed0f6a57 18 BLACK - GND
emcu 0:4192ed0f6a57 19 ORANGE - PWM
emcu 0:4192ed0f6a57 20
emcu 0:4192ed0f6a57 21 Duty - example: 1.9 (mS)
emcu 0:4192ed0f6a57 22 \ /
emcu 0:4192ed0f6a57 23 \/
emcu 0:4192ed0f6a57 24 __
emcu 0:4192ed0f6a57 25 __| |_____________
emcu 0:4192ed0f6a57 26 ... 20mS......
emcu 0:4192ed0f6a57 27
emcu 0:4192ed0f6a57 28 For rotate to right use value 1 for Duty
emcu 0:4192ed0f6a57 29 For rotate to left use value 5 for Duty
emcu 0:4192ed0f6a57 30
emcu 0:4192ed0f6a57 31 Vedere queste spiegazioni:
emcu 0:4192ed0f6a57 32 http://www.adrirobot.it/servotester/il_servomotore.htm
emcu 0:4192ed0f6a57 33
emcu 0:4192ed0f6a57 34
emcu 0:4192ed0f6a57 35
emcu 0:4192ed0f6a57 36 NOTE:
emcu 0:4192ed0f6a57 37
emcu 0:4192ed0f6a57 38 For rotate to Right (DESTRA) use this command:
emcu 0:4192ed0f6a57 39 Move(Right);
emcu 0:4192ed0f6a57 40 For rotate to Left (SINISTRA) use this command:
emcu 0:4192ed0f6a57 41 Move(Left);
emcu 0:4192ed0f6a57 42
emcu 0:4192ed0f6a57 43 You also has the possibility to send a command like below:
emcu 0:4192ed0f6a57 44 Move(1.5):
emcu 0:4192ed0f6a57 45
emcu 0:4192ed0f6a57 46 */
emcu 0:4192ed0f6a57 47
emcu 0:4192ed0f6a57 48 #include "mbed.h"
emcu 0:4192ed0f6a57 49
emcu 0:4192ed0f6a57 50 DigitalOut Drive(PA_10); // RC apre scatola
emcu 0:4192ed0f6a57 51 DigitalOut DriveI(PB_3); // RC spegne interruttore
emcu 0:4192ed0f6a57 52 DigitalIn IN1(PB_5); // Ingresso Interruttore
emcu 0:4192ed0f6a57 53 DigitalOut myled(LED1);
emcu 0:4192ed0f6a57 54 DigitalOut LedRUN(PB_4);
emcu 0:4192ed0f6a57 55
emcu 0:4192ed0f6a57 56 #define PWMoff 20 // Waiting for end PWD
emcu 0:4192ed0f6a57 57 #define Right 1 // Move to Right -> DESTRA or DX
emcu 0:4192ed0f6a57 58 #define Left 5 // Move to LEFT <- SINISTRA or SX
emcu 0:4192ed0f6a57 59 #define DLY 0.5
emcu 0:4192ed0f6a57 60
emcu 0:4192ed0f6a57 61 void Move(double);
emcu 0:4192ed0f6a57 62 void MovDiTest(void);
emcu 0:4192ed0f6a57 63 void SpegniInterruttore(void);
emcu 0:4192ed0f6a57 64 void FintoSpegniInterruttore(void);
emcu 0:4192ed0f6a57 65
emcu 0:4192ed0f6a57 66 void Seq1(void);
emcu 0:4192ed0f6a57 67 void Seq2(void);
emcu 0:4192ed0f6a57 68 void Seq3(void);
emcu 0:4192ed0f6a57 69 void Seq4(void);
emcu 0:4192ed0f6a57 70 void Seq5(void);
emcu 0:4192ed0f6a57 71 void Seq6(void);
emcu 0:4192ed0f6a57 72
emcu 0:4192ed0f6a57 73 double p=0;
emcu 0:4192ed0f6a57 74
emcu 0:4192ed0f6a57 75 void MoveI(double ON)
emcu 0:4192ed0f6a57 76 {
emcu 0:4192ed0f6a57 77 DriveI=1;
emcu 0:4192ed0f6a57 78 wait_ms(ON);
emcu 0:4192ed0f6a57 79 DriveI=0;
emcu 0:4192ed0f6a57 80 wait_ms(20);
emcu 0:4192ed0f6a57 81 }
emcu 0:4192ed0f6a57 82
emcu 0:4192ed0f6a57 83
emcu 0:4192ed0f6a57 84 void Move(double ON)
emcu 0:4192ed0f6a57 85 {
emcu 0:4192ed0f6a57 86 Drive=1;
emcu 0:4192ed0f6a57 87 wait_ms(ON);
emcu 0:4192ed0f6a57 88 Drive=0;
emcu 0:4192ed0f6a57 89 wait_ms(20);
emcu 0:4192ed0f6a57 90 }
emcu 0:4192ed0f6a57 91
emcu 0:4192ed0f6a57 92 int main()
emcu 0:4192ed0f6a57 93 {
emcu 0:4192ed0f6a57 94 int NumeroDiAperture=0;
emcu 0:4192ed0f6a57 95 int n = 0;
emcu 0:4192ed0f6a57 96
emcu 0:4192ed0f6a57 97 IN1.mode(PullUp);
emcu 0:4192ed0f6a57 98
emcu 0:4192ed0f6a57 99 printf("By: www.emcu.eu - Ver.1.0 \n\r");
emcu 0:4192ed0f6a57 100
emcu 0:4192ed0f6a57 101 // Porta motore Interruttore in posizione di RESET
emcu 0:4192ed0f6a57 102 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 103 // Sposta tutto a SX
emcu 0:4192ed0f6a57 104 for(n=0; n<30; n++)
emcu 0:4192ed0f6a57 105 {
emcu 0:4192ed0f6a57 106 MoveI(Left);
emcu 0:4192ed0f6a57 107 LedRUN = !LedRUN;
emcu 0:4192ed0f6a57 108 }
emcu 0:4192ed0f6a57 109 wait(DLY);
emcu 0:4192ed0f6a57 110 LedRUN = 1;
emcu 0:4192ed0f6a57 111
emcu 0:4192ed0f6a57 112 // Porta motore Apertura Scatola in posizione di RESET
emcu 0:4192ed0f6a57 113 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 114 // Sposta tutto a SX
emcu 0:4192ed0f6a57 115 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 116 Move(Left);
emcu 0:4192ed0f6a57 117
emcu 0:4192ed0f6a57 118 while(1)
emcu 0:4192ed0f6a57 119 {
emcu 0:4192ed0f6a57 120 if (IN1 == 1)
emcu 0:4192ed0f6a57 121 { // Button is pressed
emcu 0:4192ed0f6a57 122 //myled = 1;
emcu 0:4192ed0f6a57 123 LedRUN = 1;
emcu 0:4192ed0f6a57 124 printf("Start sequenza \n\r");
emcu 0:4192ed0f6a57 125 wait(1);
emcu 0:4192ed0f6a57 126 NumeroDiAperture++;
emcu 0:4192ed0f6a57 127
emcu 0:4192ed0f6a57 128 if (NumeroDiAperture==1)
emcu 0:4192ed0f6a57 129 Seq1();
emcu 0:4192ed0f6a57 130 if (NumeroDiAperture==2)
emcu 0:4192ed0f6a57 131 Seq2();
emcu 0:4192ed0f6a57 132 if (NumeroDiAperture==3)
emcu 0:4192ed0f6a57 133 Seq3();
emcu 0:4192ed0f6a57 134 if (NumeroDiAperture==4)
emcu 0:4192ed0f6a57 135 Seq4();
emcu 0:4192ed0f6a57 136 if (NumeroDiAperture==5)
emcu 0:4192ed0f6a57 137 Seq5();
emcu 0:4192ed0f6a57 138 if (NumeroDiAperture==6)
emcu 0:4192ed0f6a57 139 Seq6();
emcu 0:4192ed0f6a57 140
emcu 0:4192ed0f6a57 141 if (NumeroDiAperture==6)
emcu 0:4192ed0f6a57 142 NumeroDiAperture=0;
emcu 0:4192ed0f6a57 143
emcu 0:4192ed0f6a57 144 // MovDiTest();
emcu 0:4192ed0f6a57 145 }
emcu 0:4192ed0f6a57 146 else
emcu 0:4192ed0f6a57 147 // myled = 0;
emcu 0:4192ed0f6a57 148 LedRUN = 0;
emcu 0:4192ed0f6a57 149 }
emcu 0:4192ed0f6a57 150
emcu 0:4192ed0f6a57 151
emcu 0:4192ed0f6a57 152 // MovDiTest();
emcu 0:4192ed0f6a57 153
emcu 0:4192ed0f6a57 154 }
emcu 0:4192ed0f6a57 155
emcu 0:4192ed0f6a57 156
emcu 0:4192ed0f6a57 157 void Seq1(void)
emcu 0:4192ed0f6a57 158 {
emcu 0:4192ed0f6a57 159 int n=0;
emcu 0:4192ed0f6a57 160
emcu 0:4192ed0f6a57 161 printf("\n\rSequenza n.1\n\r");
emcu 0:4192ed0f6a57 162 printf("Move to 11 times to ->\n\r");
emcu 0:4192ed0f6a57 163 for(n=0; n<11; n++)
emcu 0:4192ed0f6a57 164 Move(Right);
emcu 0:4192ed0f6a57 165 wait(DLY);
emcu 0:4192ed0f6a57 166 SpegniInterruttore();
emcu 0:4192ed0f6a57 167
emcu 0:4192ed0f6a57 168 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 169 // Sposta tutto a SX
emcu 0:4192ed0f6a57 170 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 171 Move(Left);
emcu 0:4192ed0f6a57 172 printf("FINE - Sequenza n.1\n\r");
emcu 0:4192ed0f6a57 173 }
emcu 0:4192ed0f6a57 174
emcu 0:4192ed0f6a57 175
emcu 0:4192ed0f6a57 176 void Seq2(void)
emcu 0:4192ed0f6a57 177 {
emcu 0:4192ed0f6a57 178 int n=0;
emcu 0:4192ed0f6a57 179
emcu 0:4192ed0f6a57 180 printf("\n\rSequenza n.2\n\r");
emcu 0:4192ed0f6a57 181 printf("Move to 11 times to ->\n\r");
emcu 0:4192ed0f6a57 182 for(n=0; n<11; n++)
emcu 0:4192ed0f6a57 183 Move(Right);
emcu 0:4192ed0f6a57 184 wait(0.1);
emcu 0:4192ed0f6a57 185 SpegniInterruttore();
emcu 0:4192ed0f6a57 186 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 187 // Sposta tutto a SX
emcu 0:4192ed0f6a57 188 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 189 Move(Left);
emcu 0:4192ed0f6a57 190
emcu 0:4192ed0f6a57 191 printf("Move to 9 times to ->\n\r");
emcu 0:4192ed0f6a57 192 for(n=0; n<9; n++)
emcu 0:4192ed0f6a57 193 Move(Right);
emcu 0:4192ed0f6a57 194 wait(0.5);
emcu 0:4192ed0f6a57 195 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 196 // Sposta tutto a SX
emcu 0:4192ed0f6a57 197 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 198 Move(Left);
emcu 0:4192ed0f6a57 199 wait(DLY);
emcu 0:4192ed0f6a57 200
emcu 0:4192ed0f6a57 201 printf("\n\rSequenza n.2\n\r");
emcu 0:4192ed0f6a57 202 printf("Move to 11 times to ->\n\r");
emcu 0:4192ed0f6a57 203 for(n=0; n<11; n++)
emcu 0:4192ed0f6a57 204 Move(Right);
emcu 0:4192ed0f6a57 205 wait(DLY);
emcu 0:4192ed0f6a57 206 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 207 // Sposta tutto a SX
emcu 0:4192ed0f6a57 208 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 209 Move(Left);
emcu 0:4192ed0f6a57 210 printf("FINE - Sequenza n.2\n\r");
emcu 0:4192ed0f6a57 211 }
emcu 0:4192ed0f6a57 212
emcu 0:4192ed0f6a57 213
emcu 0:4192ed0f6a57 214 void Seq3(void)
emcu 0:4192ed0f6a57 215 {
emcu 0:4192ed0f6a57 216 int n=0;
emcu 0:4192ed0f6a57 217
emcu 0:4192ed0f6a57 218 printf("\n\rSequenza n.3\n\r");
emcu 0:4192ed0f6a57 219 printf("Move to 11 times to ->\n\r");
emcu 0:4192ed0f6a57 220 for(n=0; n<11; n++)
emcu 0:4192ed0f6a57 221 Move(Right);
emcu 0:4192ed0f6a57 222 wait(0.1);
emcu 0:4192ed0f6a57 223 SpegniInterruttore();
emcu 0:4192ed0f6a57 224 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 225 // Sposta tutto a SX
emcu 0:4192ed0f6a57 226 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 227 Move(Left);
emcu 0:4192ed0f6a57 228
emcu 0:4192ed0f6a57 229 printf("Move to 9 times to ->\n\r");
emcu 0:4192ed0f6a57 230 for(n=0; n<9; n++)
emcu 0:4192ed0f6a57 231 Move(Right);
emcu 0:4192ed0f6a57 232 wait(0.5);
emcu 0:4192ed0f6a57 233 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 234 // Sposta tutto a SX
emcu 0:4192ed0f6a57 235 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 236 Move(Left);
emcu 0:4192ed0f6a57 237 wait(DLY);
emcu 0:4192ed0f6a57 238
emcu 0:4192ed0f6a57 239 printf("Move to 7 times to ->\n\r");
emcu 0:4192ed0f6a57 240 for(n=0; n<7; n++)
emcu 0:4192ed0f6a57 241 Move(Right);
emcu 0:4192ed0f6a57 242 wait(0.5);
emcu 0:4192ed0f6a57 243 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 244 // Sposta tutto a SX
emcu 0:4192ed0f6a57 245 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 246 Move(Left);
emcu 0:4192ed0f6a57 247 wait(DLY);
emcu 0:4192ed0f6a57 248 printf("FINE - Sequenza n.3\n\r");
emcu 0:4192ed0f6a57 249 }
emcu 0:4192ed0f6a57 250
emcu 0:4192ed0f6a57 251
emcu 0:4192ed0f6a57 252 void Seq4(void)
emcu 0:4192ed0f6a57 253 {
emcu 0:4192ed0f6a57 254 int n=0;
emcu 0:4192ed0f6a57 255 int m=0;
emcu 0:4192ed0f6a57 256
emcu 0:4192ed0f6a57 257 printf("\n\rSequenza n.4\n\r");
emcu 0:4192ed0f6a57 258 printf("Move to 11 times to ->\n\r");
emcu 0:4192ed0f6a57 259 for(n=0; n<11; n++)
emcu 0:4192ed0f6a57 260 Move(Right);
emcu 0:4192ed0f6a57 261 wait(0.1);
emcu 0:4192ed0f6a57 262 SpegniInterruttore();
emcu 0:4192ed0f6a57 263 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 264 // Sposta tutto a SX
emcu 0:4192ed0f6a57 265 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 266 Move(Left);
emcu 0:4192ed0f6a57 267
emcu 0:4192ed0f6a57 268 wait(0.2);
emcu 0:4192ed0f6a57 269 for (m=0; m<5; m++)
emcu 0:4192ed0f6a57 270 {
emcu 0:4192ed0f6a57 271 printf("Move to 9 times to ->\n\r");
emcu 0:4192ed0f6a57 272 for(n=0; n<8; n++)
emcu 0:4192ed0f6a57 273 Move(Right);
emcu 0:4192ed0f6a57 274 wait(0.2);
emcu 0:4192ed0f6a57 275 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 276 // Sposta tutto a SX
emcu 0:4192ed0f6a57 277 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 278 Move(Left);
emcu 0:4192ed0f6a57 279
emcu 0:4192ed0f6a57 280 //wait(0.05);
emcu 0:4192ed0f6a57 281 }
emcu 0:4192ed0f6a57 282 printf("FINE - Sequenza n.4\n\r");
emcu 0:4192ed0f6a57 283 }
emcu 0:4192ed0f6a57 284
emcu 0:4192ed0f6a57 285
emcu 0:4192ed0f6a57 286 void Seq5(void)
emcu 0:4192ed0f6a57 287 {
emcu 0:4192ed0f6a57 288 int n=0;
emcu 0:4192ed0f6a57 289 int m=0;
emcu 0:4192ed0f6a57 290
emcu 0:4192ed0f6a57 291 printf("\n\rSequenza n.5\n\r");
emcu 0:4192ed0f6a57 292 printf("Move to 11 times to ->\n\r");
emcu 0:4192ed0f6a57 293 for(n=0; n<11; n++)
emcu 0:4192ed0f6a57 294 Move(Right);
emcu 0:4192ed0f6a57 295 wait(0.1);
emcu 0:4192ed0f6a57 296 SpegniInterruttore();
emcu 0:4192ed0f6a57 297 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 298 // Sposta tutto a SX
emcu 0:4192ed0f6a57 299 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 300 Move(Left);
emcu 0:4192ed0f6a57 301
emcu 0:4192ed0f6a57 302 wait(0.2);
emcu 0:4192ed0f6a57 303 for (m=0; m<5; m++)
emcu 0:4192ed0f6a57 304 {
emcu 0:4192ed0f6a57 305 printf("Move to 9 times to ->\n\r");
emcu 0:4192ed0f6a57 306 for(n=0; n<8; n++)
emcu 0:4192ed0f6a57 307 Move(Right);
emcu 0:4192ed0f6a57 308 wait(0.2);
emcu 0:4192ed0f6a57 309 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 310 // Sposta tutto a SX
emcu 0:4192ed0f6a57 311 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 312 Move(Left);
emcu 0:4192ed0f6a57 313
emcu 0:4192ed0f6a57 314 //wait(0.05);
emcu 0:4192ed0f6a57 315 }
emcu 0:4192ed0f6a57 316
emcu 0:4192ed0f6a57 317 FintoSpegniInterruttore();
emcu 0:4192ed0f6a57 318
emcu 0:4192ed0f6a57 319 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 320 // Sposta tutto a SX
emcu 0:4192ed0f6a57 321 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 322 Move(Left);
emcu 0:4192ed0f6a57 323
emcu 0:4192ed0f6a57 324 printf("FINE - Sequenza n.5\n\r");
emcu 0:4192ed0f6a57 325 }
emcu 0:4192ed0f6a57 326
emcu 0:4192ed0f6a57 327
emcu 0:4192ed0f6a57 328 void Seq6(void)
emcu 0:4192ed0f6a57 329 {
emcu 0:4192ed0f6a57 330 int n=0;
emcu 0:4192ed0f6a57 331
emcu 0:4192ed0f6a57 332 printf("\n\rSequenza n.6\n\r");
emcu 0:4192ed0f6a57 333 printf("Move to 11 times to ->\n\r");
emcu 0:4192ed0f6a57 334 for(n=0; n<11; n++)
emcu 0:4192ed0f6a57 335 {
emcu 0:4192ed0f6a57 336 Move(Right);
emcu 0:4192ed0f6a57 337 wait(0.1);
emcu 0:4192ed0f6a57 338 }
emcu 0:4192ed0f6a57 339 wait(DLY);
emcu 0:4192ed0f6a57 340 SpegniInterruttore();
emcu 0:4192ed0f6a57 341 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 342 // Sposta tutto a SX
emcu 0:4192ed0f6a57 343 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 344 Move(Left);
emcu 0:4192ed0f6a57 345 wait(DLY);
emcu 0:4192ed0f6a57 346
emcu 0:4192ed0f6a57 347 printf("Move to 8 times to ->\n\r");
emcu 0:4192ed0f6a57 348 for(n=0; n<8; n++)
emcu 0:4192ed0f6a57 349 {
emcu 0:4192ed0f6a57 350 Move(Right);
emcu 0:4192ed0f6a57 351 }
emcu 0:4192ed0f6a57 352 wait(1.5);
emcu 0:4192ed0f6a57 353 printf("Move to 3 times to ->\n\r");
emcu 0:4192ed0f6a57 354 for(n=0; n<3; n++)
emcu 0:4192ed0f6a57 355 {
emcu 0:4192ed0f6a57 356 Move(Right);
emcu 0:4192ed0f6a57 357 }
emcu 0:4192ed0f6a57 358 wait(1.5);
emcu 0:4192ed0f6a57 359
emcu 0:4192ed0f6a57 360 printf("Move to 2 times to <-\n\r");
emcu 0:4192ed0f6a57 361 for(n=0; n<2; n++)
emcu 0:4192ed0f6a57 362 Move(Left);
emcu 0:4192ed0f6a57 363 wait(1.5);
emcu 0:4192ed0f6a57 364
emcu 0:4192ed0f6a57 365
emcu 0:4192ed0f6a57 366 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 367 // Sposta tutto a SX
emcu 0:4192ed0f6a57 368 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 369 Move(Left);
emcu 0:4192ed0f6a57 370
emcu 0:4192ed0f6a57 371
emcu 0:4192ed0f6a57 372 printf("FINE - Sequenza n.6\n\r");
emcu 0:4192ed0f6a57 373 }
emcu 0:4192ed0f6a57 374
emcu 0:4192ed0f6a57 375
emcu 0:4192ed0f6a57 376
emcu 0:4192ed0f6a57 377
emcu 0:4192ed0f6a57 378
emcu 0:4192ed0f6a57 379 void SpegniInterruttore(void)
emcu 0:4192ed0f6a57 380 {
emcu 0:4192ed0f6a57 381 int n=0;
emcu 0:4192ed0f6a57 382
emcu 0:4192ed0f6a57 383 // Movimento per INTERRUTTORE
emcu 0:4192ed0f6a57 384
emcu 0:4192ed0f6a57 385 printf("Move to 21 times to -> \n\r");
emcu 0:4192ed0f6a57 386 // Sposta tutto a SX
emcu 0:4192ed0f6a57 387 for(n=0; n<21; n++)
emcu 0:4192ed0f6a57 388 {
emcu 0:4192ed0f6a57 389 MoveI(Right);
emcu 0:4192ed0f6a57 390 LedRUN = !LedRUN;
emcu 0:4192ed0f6a57 391 }
emcu 0:4192ed0f6a57 392 LedRUN = 1;
emcu 0:4192ed0f6a57 393 wait(1.5);
emcu 0:4192ed0f6a57 394 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 395 // Sposta tutto a SX
emcu 0:4192ed0f6a57 396 for(n=0; n<30; n++)
emcu 0:4192ed0f6a57 397 {
emcu 0:4192ed0f6a57 398 MoveI(Left);
emcu 0:4192ed0f6a57 399 LedRUN = !LedRUN;
emcu 0:4192ed0f6a57 400 }
emcu 0:4192ed0f6a57 401 wait(DLY);
emcu 0:4192ed0f6a57 402 LedRUN = 1;
emcu 0:4192ed0f6a57 403 }
emcu 0:4192ed0f6a57 404
emcu 0:4192ed0f6a57 405
emcu 0:4192ed0f6a57 406 void FintoSpegniInterruttore(void)
emcu 0:4192ed0f6a57 407 {
emcu 0:4192ed0f6a57 408 int n=0;
emcu 0:4192ed0f6a57 409
emcu 0:4192ed0f6a57 410 printf("Finto spegni Interruttore \n\r");
emcu 0:4192ed0f6a57 411 printf("Move to 11 times to ->\n\r");
emcu 0:4192ed0f6a57 412 for(n=0; n<11; n++)
emcu 0:4192ed0f6a57 413 Move(Right);
emcu 0:4192ed0f6a57 414 wait(DLY);
emcu 0:4192ed0f6a57 415 printf("Move to 10 times to -> \n\r");
emcu 0:4192ed0f6a57 416 // Sposta tutto a SX
emcu 0:4192ed0f6a57 417 for(n=0; n<10; n++)
emcu 0:4192ed0f6a57 418 MoveI(Right);
emcu 0:4192ed0f6a57 419 wait(DLY);
emcu 0:4192ed0f6a57 420
emcu 0:4192ed0f6a57 421 printf("Move to 5 times to -> \n\r");
emcu 0:4192ed0f6a57 422 // Sposta tutto a SX
emcu 0:4192ed0f6a57 423 for(n=0; n<5; n++)
emcu 0:4192ed0f6a57 424 MoveI(Right);
emcu 0:4192ed0f6a57 425 wait(DLY);
emcu 0:4192ed0f6a57 426
emcu 0:4192ed0f6a57 427 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 428 // Sposta tutto a SX
emcu 0:4192ed0f6a57 429 for(n=0; n<30; n++)
emcu 0:4192ed0f6a57 430 MoveI(Left);
emcu 0:4192ed0f6a57 431 wait(DLY);
emcu 0:4192ed0f6a57 432 }
emcu 0:4192ed0f6a57 433
emcu 0:4192ed0f6a57 434
emcu 0:4192ed0f6a57 435 void MovDiTest(void)
emcu 0:4192ed0f6a57 436 {
emcu 0:4192ed0f6a57 437 int n=0;
emcu 0:4192ed0f6a57 438 int m=0;
emcu 0:4192ed0f6a57 439
emcu 0:4192ed0f6a57 440 printf("Move to 11 times to ->\n\r");
emcu 0:4192ed0f6a57 441 for(n=0; n<11; n++)
emcu 0:4192ed0f6a57 442 Move(Right);
emcu 0:4192ed0f6a57 443 wait(DLY);
emcu 0:4192ed0f6a57 444 SpegniInterruttore();
emcu 0:4192ed0f6a57 445
emcu 0:4192ed0f6a57 446 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 447 // Sposta tutto a SX
emcu 0:4192ed0f6a57 448 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 449 Move(Left);
emcu 0:4192ed0f6a57 450
emcu 0:4192ed0f6a57 451 wait(DLY);
emcu 0:4192ed0f6a57 452
emcu 0:4192ed0f6a57 453 printf("Move to 9 times to ->\n\r");
emcu 0:4192ed0f6a57 454 for(n=0; n<9; n++)
emcu 0:4192ed0f6a57 455 Move(Right);
emcu 0:4192ed0f6a57 456 wait(0.2);
emcu 0:4192ed0f6a57 457 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 458 // Sposta tutto a SX
emcu 0:4192ed0f6a57 459 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 460 Move(Left);
emcu 0:4192ed0f6a57 461 wait(DLY);
emcu 0:4192ed0f6a57 462
emcu 0:4192ed0f6a57 463 printf("Move to 7 times to ->\n\r");
emcu 0:4192ed0f6a57 464 for(n=0; n<7; n++)
emcu 0:4192ed0f6a57 465 Move(Right);
emcu 0:4192ed0f6a57 466 wait(DLY);
emcu 0:4192ed0f6a57 467
emcu 0:4192ed0f6a57 468 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 469 // Sposta tutto a SX
emcu 0:4192ed0f6a57 470 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 471 Move(Left);
emcu 0:4192ed0f6a57 472 wait(DLY);
emcu 0:4192ed0f6a57 473
emcu 0:4192ed0f6a57 474 for (m=0; m<5; m++)
emcu 0:4192ed0f6a57 475 {
emcu 0:4192ed0f6a57 476 printf("Move to 9 times to ->\n\r");
emcu 0:4192ed0f6a57 477 for(n=0; n<8; n++)
emcu 0:4192ed0f6a57 478 Move(Right);
emcu 0:4192ed0f6a57 479
emcu 0:4192ed0f6a57 480 wait(0.2);
emcu 0:4192ed0f6a57 481
emcu 0:4192ed0f6a57 482 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 483 // Sposta tutto a SX
emcu 0:4192ed0f6a57 484 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 485 Move(Left);
emcu 0:4192ed0f6a57 486
emcu 0:4192ed0f6a57 487 //wait(0.05);
emcu 0:4192ed0f6a57 488 }
emcu 0:4192ed0f6a57 489
emcu 0:4192ed0f6a57 490 wait(DLY);
emcu 0:4192ed0f6a57 491
emcu 0:4192ed0f6a57 492 printf("Move to 9 times to ->\n\r");
emcu 0:4192ed0f6a57 493 for(n=0; n<9; n++)
emcu 0:4192ed0f6a57 494 Move(Right);
emcu 0:4192ed0f6a57 495 wait(DLY);
emcu 0:4192ed0f6a57 496
emcu 0:4192ed0f6a57 497 printf("Rotate all to <- \n\r");
emcu 0:4192ed0f6a57 498 // Sposta tutto a SX
emcu 0:4192ed0f6a57 499 for(n=0; n<20; n++)
emcu 0:4192ed0f6a57 500 Move(Left);
emcu 0:4192ed0f6a57 501 wait(DLY);
emcu 0:4192ed0f6a57 502 }
emcu 0:4192ed0f6a57 503
emcu 0:4192ed0f6a57 504
emcu 0:4192ed0f6a57 505 /*
emcu 0:4192ed0f6a57 506
emcu 0:4192ed0f6a57 507 // Sposta tutto a DX - Move all to RIGHT
emcu 0:4192ed0f6a57 508 for(n=0; n<200; n++)
emcu 0:4192ed0f6a57 509 Move(Right);
emcu 0:4192ed0f6a57 510
emcu 0:4192ed0f6a57 511 wait(DLY);
emcu 0:4192ed0f6a57 512
emcu 0:4192ed0f6a57 513 // Sposta tutto a SX - Move all to LEFT
emcu 0:4192ed0f6a57 514 for(n=0; n<200; n++)
emcu 0:4192ed0f6a57 515 Move(Left);
emcu 0:4192ed0f6a57 516
emcu 0:4192ed0f6a57 517 wait(DLY);
emcu 0:4192ed0f6a57 518
emcu 0:4192ed0f6a57 519 */
emcu 0:4192ed0f6a57 520
emcu 0:4192ed0f6a57 521 /*
emcu 0:4192ed0f6a57 522 p=0.4;
emcu 0:4192ed0f6a57 523 myservo = p;
emcu 0:4192ed0f6a57 524 printf("P = %.2f \n\r", p);
emcu 0:4192ed0f6a57 525 wait(0.2);
emcu 0:4192ed0f6a57 526 */