sebastian martinez / Mbed 2 deprecated Movimiento_8Servos

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

Fork of 01-04_Primer_Avance by sebastian martinez

mover.cpp@5:6ca97bfe9406, 2018-10-06 (annotated)

Committer:
sebasmartinez
Date:
Sat Oct 06 17:45:42 2018 +0000
Revision:
5:6ca97bfe9406
Parent:
4:ba51736778e1 
Programa Movimiento 1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fabeltranm 1:526bdd5faa37 1 #include "mover.h"
fabeltranm 0:89b318e49395 2 #include "mbed.h"
fabeltranm 0:89b318e49395 3 #include "math.h"
fabeltranm 0:89b318e49395 4
sebasmartinez 5:6ca97bfe9406 5 PwmOut Servo1(PB_14); //ROJO
fabeltranm 0:89b318e49395 6
sebasmartinez 5:6ca97bfe9406 7 PwmOut Servo2(PB_0); //BLANCO
sebasmartinez 5:6ca97bfe9406 8
sebasmartinez 5:6ca97bfe9406 9 PwmOut Servo3(PA_11); //VERDE
sebasmartinez 5:6ca97bfe9406 10 PwmOut Servo4(PA_8); //NEGRO
sebasmartinez 5:6ca97bfe9406 11 PwmOut Servo5(PB_6); //NARANJA
sebasmartinez 5:6ca97bfe9406 12
sebasmartinez 5:6ca97bfe9406 13 PwmOut Servo6(PB_1); //CAFE
fabeltranm 0:89b318e49395 14
sebasmartinez 5:6ca97bfe9406 15 PwmOut Servo7(PA_10); //AZUL
sebasmartinez 5:6ca97bfe9406 16
sebasmartinez 5:6ca97bfe9406 17 PwmOut Servo8(PA_6); //AMARILLO
sebasmartinez 5:6ca97bfe9406 18
sebasmartinez 5:6ca97bfe9406 19 /*
sebasmartinez 5:6ca97bfe9406 20 PwmOut Servo6(PB_1); //CAFE
sebasmartinez 5:6ca97bfe9406 21 PwmOut Servo7(PA_7); //AZUL
sebasmartinez 5:6ca97bfe9406 22 PwmOut Servo8(PA_6); //AMARILLO
fabeltranm 1:526bdd5faa37 23
fabeltranm 0:89b318e49395 24 uint8_t ss_time=50; // tiempo de espera para moverse 1 mm en microsegundos
fabeltranm 0:89b318e49395 25
fabeltranm 0:89b318e49395 26 void put_sstime(uint8_t vtime){
fabeltranm 0:89b318e49395 27 ss_time=vtime;
fabeltranm 0:89b318e49395 28
fabeltranm 0:89b318e49395 29 }
sebasmartinez 5:6ca97bfe9406 30 */
fabeltranm 0:89b318e49395 31 int coord2us(float coord)
fabeltranm 0:89b318e49395 32 {
fabeltranm 0:89b318e49395 33 if(0 <= coord <= MAXPOS)
sebasmartinez 5:6ca97bfe9406 34 { return int(500+coord*2000/180);// u6
sebasmartinez 5:6ca97bfe9406 35 }
sebasmartinez 3:60722da62531 36 return 500;
sebasmartinez 3:60722da62531 37 /*
sebasmartinez 3:60722da62531 38 Secuencia 0, 180, 135, 90, 45, 0.
sebasmartinez 3:60722da62531 39 FF 01 01 00 FE FF 01 01 B4 FE FF 01 01 87 FE FF 01 01 5A FE FF 01 01 2D FE FF 01 01 00 FE
sebasmartinez 3:60722da62531 40 Grados: 180-B4, 135-87, 90-5A, 45-2D, 0-00;
sebasmartinez 3:60722da62531 41 */
fabeltranm 0:89b318e49395 42 }
fabeltranm 0:89b318e49395 43
fabeltranm 1:526bdd5faa37 44
fabeltranm 0:89b318e49395 45
fabeltranm 0:89b318e49395 46
fabeltranm 0:89b318e49395 47
fabeltranm 1:526bdd5faa37 48 void mover_ser(uint8_t motor, uint8_t pos){
fabeltranm 0:89b318e49395 49
sebasmartinez 3:60722da62531 50 int pulseX = coord2us(pos);
sebasmartinez 4:ba51736778e1 51 //int pulseX = 900;
sebasmartinez 4:ba51736778e1 52 //int pulseY = 1500;
sebasmartinez 4:ba51736778e1 53 // myServo1.pulsewidth_us(pulseX);
sebasmartinez 2:10956b8ceffb 54 switch ( motor ) {
sebasmartinez 2:10956b8ceffb 55
sebasmartinez 2:10956b8ceffb 56 case 0x01:
sebasmartinez 2:10956b8ceffb 57 Servo1.pulsewidth_us(pulseX);
sebasmartinez 2:10956b8ceffb 58 break;
sebasmartinez 2:10956b8ceffb 59 case 0x02:
sebasmartinez 3:60722da62531 60 Servo2.pulsewidth_us(pulseX);
sebasmartinez 2:10956b8ceffb 61 break;
sebasmartinez 2:10956b8ceffb 62 case 0x03:
sebasmartinez 2:10956b8ceffb 63 Servo3.pulsewidth_us(pulseX);
sebasmartinez 2:10956b8ceffb 64 break;
sebasmartinez 2:10956b8ceffb 65 case 0x04:
sebasmartinez 2:10956b8ceffb 66 Servo4.pulsewidth_us(pulseX);
sebasmartinez 2:10956b8ceffb 67 break;
sebasmartinez 2:10956b8ceffb 68 case 0x05:
sebasmartinez 2:10956b8ceffb 69 Servo5.pulsewidth_us(pulseX);
sebasmartinez 2:10956b8ceffb 70 break;
sebasmartinez 2:10956b8ceffb 71 case 0x06:
sebasmartinez 2:10956b8ceffb 72 Servo6.pulsewidth_us(pulseX);
sebasmartinez 2:10956b8ceffb 73 break;
sebasmartinez 2:10956b8ceffb 74 case 0x07:
sebasmartinez 2:10956b8ceffb 75 Servo7.pulsewidth_us(pulseX);
sebasmartinez 2:10956b8ceffb 76 break;
sebasmartinez 2:10956b8ceffb 77 case 0x08:
sebasmartinez 2:10956b8ceffb 78 Servo8.pulsewidth_us(pulseX);
sebasmartinez 2:10956b8ceffb 79 break;
sebasmartinez 2:10956b8ceffb 80
sebasmartinez 2:10956b8ceffb 81 default: break;
sebasmartinez 2:10956b8ceffb 82 }
fabeltranm 0:89b318e49395 83
fabeltranm 0:89b318e49395 84 }
sebasmartinez 4:ba51736778e1 85 void mover_par(uint8_t pares, uint8_t dir){
sebasmartinez 4:ba51736778e1 86
sebasmartinez 4:ba51736778e1 87 switch ( pares ) {
sebasmartinez 4:ba51736778e1 88
sebasmartinez 4:ba51736778e1 89 case 0x01:
sebasmartinez 4:ba51736778e1 90 if(dir==0x01){
sebasmartinez 4:ba51736778e1 91 Servo1.pulsewidth_us(ARRIBA);
sebasmartinez 4:ba51736778e1 92 wait(0.5);
sebasmartinez 4:ba51736778e1 93 Servo2.pulsewidth_us(ADELANTE);
sebasmartinez 4:ba51736778e1 94 wait(0.5);
sebasmartinez 4:ba51736778e1 95 Servo1.pulsewidth_us(ABAJO);
sebasmartinez 4:ba51736778e1 96 wait(0.5);
sebasmartinez 4:ba51736778e1 97 Servo2.pulsewidth_us(ATRAS);
sebasmartinez 4:ba51736778e1 98 wait(0.5);
sebasmartinez 4:ba51736778e1 99 }else{
sebasmartinez 4:ba51736778e1 100 Servo1.pulsewidth_us(ARRIBA);
sebasmartinez 4:ba51736778e1 101 wait(0.5);
sebasmartinez 4:ba51736778e1 102 Servo2.pulsewidth_us(ATRAS);
sebasmartinez 4:ba51736778e1 103 wait(0.5);
sebasmartinez 4:ba51736778e1 104 Servo1.pulsewidth_us(ABAJO);
sebasmartinez 4:ba51736778e1 105 wait(0.5);
sebasmartinez 4:ba51736778e1 106 Servo2.pulsewidth_us(ADELANTE);
sebasmartinez 4:ba51736778e1 107 wait(0.5);
sebasmartinez 4:ba51736778e1 108 }
sebasmartinez 4:ba51736778e1 109 break;
sebasmartinez 5:6ca97bfe9406 110 case 0x02:
sebasmartinez 5:6ca97bfe9406 111 if(dir==0x01){
sebasmartinez 5:6ca97bfe9406 112 Servo3.pulsewidth_us(ARRIBA);
sebasmartinez 4:ba51736778e1 113 wait(0.5);
sebasmartinez 5:6ca97bfe9406 114 Servo4.pulsewidth_us(ADELANTE);
sebasmartinez 4:ba51736778e1 115 wait(0.5);
sebasmartinez 5:6ca97bfe9406 116 Servo3.pulsewidth_us(ABAJO);
sebasmartinez 4:ba51736778e1 117 wait(0.5);
sebasmartinez 5:6ca97bfe9406 118 Servo4.pulsewidth_us(ATRAS);
sebasmartinez 4:ba51736778e1 119 wait(0.5);
sebasmartinez 4:ba51736778e1 120 }else{
sebasmartinez 5:6ca97bfe9406 121 Servo3.pulsewidth_us(ARRIBA);
sebasmartinez 4:ba51736778e1 122 wait(0.5);
sebasmartinez 5:6ca97bfe9406 123 Servo4.pulsewidth_us(ATRAS);
sebasmartinez 4:ba51736778e1 124 wait(0.5);
sebasmartinez 5:6ca97bfe9406 125 Servo3.pulsewidth_us(ABAJO);
sebasmartinez 4:ba51736778e1 126 wait(0.5);
sebasmartinez 5:6ca97bfe9406 127 Servo4.pulsewidth_us(ADELANTE);
sebasmartinez 4:ba51736778e1 128 wait(0.5);
sebasmartinez 4:ba51736778e1 129 }
sebasmartinez 4:ba51736778e1 130 break;
sebasmartinez 5:6ca97bfe9406 131 case 0x03:
sebasmartinez 5:6ca97bfe9406 132 if(dir==0x01){
sebasmartinez 5:6ca97bfe9406 133 Servo5.pulsewidth_us(ARRIBA);
sebasmartinez 4:ba51736778e1 134 wait(0.5);
sebasmartinez 5:6ca97bfe9406 135 Servo6.pulsewidth_us(ADELANTE);
sebasmartinez 4:ba51736778e1 136 wait(0.5);
sebasmartinez 5:6ca97bfe9406 137 Servo5.pulsewidth_us(ABAJO);
sebasmartinez 4:ba51736778e1 138 wait(0.5);
sebasmartinez 5:6ca97bfe9406 139 Servo6.pulsewidth_us(ATRAS);
sebasmartinez 4:ba51736778e1 140 wait(0.5);
sebasmartinez 4:ba51736778e1 141 }else{
sebasmartinez 5:6ca97bfe9406 142 Servo5.pulsewidth_us(ARRIBA);
sebasmartinez 4:ba51736778e1 143 wait(0.5);
sebasmartinez 5:6ca97bfe9406 144 Servo6.pulsewidth_us(ATRAS);
sebasmartinez 4:ba51736778e1 145 wait(0.5);
sebasmartinez 5:6ca97bfe9406 146 Servo5.pulsewidth_us(ABAJO);
sebasmartinez 4:ba51736778e1 147 wait(0.5);
sebasmartinez 5:6ca97bfe9406 148 Servo6.pulsewidth_us(ADELANTE);
sebasmartinez 4:ba51736778e1 149 wait(0.5);
sebasmartinez 4:ba51736778e1 150 }
sebasmartinez 4:ba51736778e1 151 break;
sebasmartinez 5:6ca97bfe9406 152 case 0x04:
sebasmartinez 5:6ca97bfe9406 153 if(dir==0x01){
sebasmartinez 5:6ca97bfe9406 154 Servo7.pulsewidth_us(ARRIBA);
sebasmartinez 5:6ca97bfe9406 155 wait(0.5);
sebasmartinez 5:6ca97bfe9406 156 Servo8.pulsewidth_us(ADELANTE);
sebasmartinez 5:6ca97bfe9406 157 wait(0.5);
sebasmartinez 5:6ca97bfe9406 158 Servo7.pulsewidth_us(ABAJO);
sebasmartinez 5:6ca97bfe9406 159 wait(0.5);
sebasmartinez 5:6ca97bfe9406 160 Servo8.pulsewidth_us(ATRAS);
sebasmartinez 5:6ca97bfe9406 161 wait(0.5);
sebasmartinez 5:6ca97bfe9406 162 }else{
sebasmartinez 5:6ca97bfe9406 163 Servo7.pulsewidth_us(ARRIBA);
sebasmartinez 5:6ca97bfe9406 164 wait(0.5);
sebasmartinez 5:6ca97bfe9406 165 Servo8.pulsewidth_us(ATRAS);
sebasmartinez 5:6ca97bfe9406 166 wait(0.5);
sebasmartinez 5:6ca97bfe9406 167 Servo7.pulsewidth_us(ABAJO);
sebasmartinez 5:6ca97bfe9406 168 wait(0.5);
sebasmartinez 5:6ca97bfe9406 169 Servo8.pulsewidth_us(ADELANTE);
sebasmartinez 5:6ca97bfe9406 170 wait(0.5);
sebasmartinez 5:6ca97bfe9406 171 }
sebasmartinez 5:6ca97bfe9406 172 break;
sebasmartinez 5:6ca97bfe9406 173
sebasmartinez 4:ba51736778e1 174 default: break;
sebasmartinez 4:ba51736778e1 175 }
sebasmartinez 4:ba51736778e1 176 }
sebasmartinez 4:ba51736778e1 177 void centro(uint8_t pos_p){
sebasmartinez 4:ba51736778e1 178 switch ( pos_p ) {
sebasmartinez 4:ba51736778e1 179
sebasmartinez 4:ba51736778e1 180 case 0x01:
sebasmartinez 4:ba51736778e1 181 Servo1.pulsewidth_us(CENTRO);
sebasmartinez 4:ba51736778e1 182 wait(1);
sebasmartinez 4:ba51736778e1 183 Servo2.pulsewidth_us(CENTRO);
sebasmartinez 4:ba51736778e1 184 wait(1);
sebasmartinez 4:ba51736778e1 185 break;
sebasmartinez 4:ba51736778e1 186 case 0x02:
sebasmartinez 4:ba51736778e1 187 Servo3.pulsewidth_us(CENTRO);
sebasmartinez 4:ba51736778e1 188 wait(1);
sebasmartinez 4:ba51736778e1 189 Servo4.pulsewidth_us(CENTRO);
sebasmartinez 4:ba51736778e1 190 wait(1);
sebasmartinez 4:ba51736778e1 191 break;
sebasmartinez 4:ba51736778e1 192 case 0x03:
sebasmartinez 4:ba51736778e1 193 Servo5.pulsewidth_us(CENTRO);
sebasmartinez 4:ba51736778e1 194 wait(1);
sebasmartinez 4:ba51736778e1 195 Servo6.pulsewidth_us(CENTRO);
sebasmartinez 4:ba51736778e1 196 wait(1);
sebasmartinez 4:ba51736778e1 197 break;
sebasmartinez 4:ba51736778e1 198 case 0x04:
sebasmartinez 4:ba51736778e1 199 Servo7.pulsewidth_us(CENTRO);
sebasmartinez 4:ba51736778e1 200 wait(1);
sebasmartinez 4:ba51736778e1 201 Servo8.pulsewidth_us(CENTRO);
sebasmartinez 4:ba51736778e1 202 wait(1);
sebasmartinez 4:ba51736778e1 203 break;
sebasmartinez 4:ba51736778e1 204 default: break;
sebasmartinez 4:ba51736778e1 205 }
sebasmartinez 4:ba51736778e1 206 }
fabeltranm 0:89b318e49395 207 void init_servo()
fabeltranm 0:89b318e49395 208 {
sebasmartinez 2:10956b8ceffb 209 Servo1.period_ms(20);
sebasmartinez 2:10956b8ceffb 210 Servo2.period_ms(20);
sebasmartinez 2:10956b8ceffb 211 Servo3.period_ms(20);
sebasmartinez 2:10956b8ceffb 212 Servo4.period_ms(20);
sebasmartinez 2:10956b8ceffb 213 Servo5.period_ms(20);
sebasmartinez 2:10956b8ceffb 214 Servo6.period_ms(20);
sebasmartinez 2:10956b8ceffb 215 Servo7.period_ms(20);
sebasmartinez 2:10956b8ceffb 216 Servo8.period_ms(20);
sebasmartinez 5:6ca97bfe9406 217 /*
sebasmartinez 5:6ca97bfe9406 218 //Posicion inicial a cada servo CENTRO
sebasmartinez 5:6ca97bfe9406 219 Servo1.pulsewidth_us(CENTRO);
sebasmartinez 5:6ca97bfe9406 220 Servo2.pulsewidth_us(CENTRO);
sebasmartinez 5:6ca97bfe9406 221 Servo3.pulsewidth_us(CENTRO);
sebasmartinez 5:6ca97bfe9406 222 Servo4.pulsewidth_us(CENTRO);
sebasmartinez 5:6ca97bfe9406 223 Servo5.pulsewidth_us(CENTRO);
sebasmartinez 5:6ca97bfe9406 224 Servo6.pulsewidth_us(CENTRO);
sebasmartinez 5:6ca97bfe9406 225 Servo7.pulsewidth_us(CENTRO);
sebasmartinez 5:6ca97bfe9406 226 Servo8.pulsewidth_us(CENTRO);
sebasmartinez 5:6ca97bfe9406 227 */
sebasmartinez 2:10956b8ceffb 228 }
fabeltranm 0:89b318e49395 229