![](/media/cache/profiles/LogoEMCU_EU_2.png.50x50_q85.jpg)
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
Fork of ScatolaMagica by
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?
User | Revision | Line number | New 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 | */ |