sebastian martinez / Mbed 2 deprecated Avance_Color

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

mover.cpp@0:73e805ecf765, 2018-10-10 (annotated)

Committer:
sebasmartinez
Date:
Wed Oct 10 22:49:18 2018 +0000
Revision:
0:73e805ecf765
Child:
1:59be7d7e1b54
ENTREGA

Who changed what in which revision?

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