sebastian martinez / Mbed 2 deprecated 01-04_Primer_Avance

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-04EntregaPrimerCorte by ferney alberto beltran molina

mover.cpp@4:ba51736778e1, 2018-09-07 (annotated)

Committer:
sebasmartinez
Date:
Fri Sep 07 23:58:14 2018 +0000
Revision:
4:ba51736778e1
Parent:
3:60722da62531 
V3

Who changed what in which revision?

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