sm moreno / Mbed 2 deprecated Entrega_Final

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:22b33429aff5, 2018-11-22 (annotated)

Committer:
sammir
Date:
Thu Nov 22 01:19:55 2018 +0000
Revision:
0:22b33429aff5
TRABAJO FINAL CUADRUPEDO

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sammir 0:22b33429aff5 1 #include "mover.h"
sammir 0:22b33429aff5 2 #include "mbed.h"
sammir 0:22b33429aff5 3 #include "math.h"
sammir 0:22b33429aff5 4
sammir 0:22b33429aff5 5 PwmOut myServo1(PB_13);//DFA
sammir 0:22b33429aff5 6 PwmOut myServo2(PB_14);//DFB
sammir 0:22b33429aff5 7 PwmOut myServo3(PB_1);//DTA
sammir 0:22b33429aff5 8 PwmOut myServo4(PB_9);//DTB
sammir 0:22b33429aff5 9 PwmOut myServo5(PB_2);//IFA
sammir 0:22b33429aff5 10 PwmOut myServo6(PA_11);//IFB
sammir 0:22b33429aff5 11 PwmOut myServo7(PC_6);//ITA
sammir 0:22b33429aff5 12 PwmOut myServo8(PC_8);//ITB
sammir 0:22b33429aff5 13
sammir 0:22b33429aff5 14
sammir 0:22b33429aff5 15
sammir 0:22b33429aff5 16
sammir 0:22b33429aff5 17 uint8_t ss_time= 255; // tiempo de espera para moverse 1 mm en microsegundos
sammir 0:22b33429aff5 18
sammir 0:22b33429aff5 19 void put_sstime(uint8_t vtime){
sammir 0:22b33429aff5 20 ss_time=vtime;
sammir 0:22b33429aff5 21
sammir 0:22b33429aff5 22 }
sammir 0:22b33429aff5 23
sammir 0:22b33429aff5 24 int coord2us(float coord)
sammir 0:22b33429aff5 25 {
sammir 0:22b33429aff5 26 if(0 <= coord <= MAXPOS)
sammir 0:22b33429aff5 27 return int(750+coord*1900/180);// u6
sammir 0:22b33429aff5 28 return 750;
sammir 0:22b33429aff5 29
sammir 0:22b33429aff5 30 }
sammir 0:22b33429aff5 31
sammir 0:22b33429aff5 32
sammir 0:22b33429aff5 33
sammir 0:22b33429aff5 34
sammir 0:22b33429aff5 35
sammir 0:22b33429aff5 36 void mover_ser(uint8_t motor, uint8_t pos){
sammir 0:22b33429aff5 37 int pulseX = coord2us(pos);
sammir 0:22b33429aff5 38 switch(motor){
sammir 0:22b33429aff5 39
sammir 0:22b33429aff5 40 case 1:
sammir 0:22b33429aff5 41
sammir 0:22b33429aff5 42 myServo1.pulsewidth_us(pulseX) ;
sammir 0:22b33429aff5 43
sammir 0:22b33429aff5 44
sammir 0:22b33429aff5 45 break;
sammir 0:22b33429aff5 46
sammir 0:22b33429aff5 47 case 2:
sammir 0:22b33429aff5 48
sammir 0:22b33429aff5 49 myServo2.pulsewidth_us(pulseX);
sammir 0:22b33429aff5 50
sammir 0:22b33429aff5 51 break;
sammir 0:22b33429aff5 52
sammir 0:22b33429aff5 53 case 3:
sammir 0:22b33429aff5 54
sammir 0:22b33429aff5 55 myServo3.pulsewidth_us(pulseX);
sammir 0:22b33429aff5 56
sammir 0:22b33429aff5 57 break;
sammir 0:22b33429aff5 58
sammir 0:22b33429aff5 59 case 4:
sammir 0:22b33429aff5 60
sammir 0:22b33429aff5 61 myServo4.pulsewidth_us(pulseX);
sammir 0:22b33429aff5 62
sammir 0:22b33429aff5 63 break;
sammir 0:22b33429aff5 64
sammir 0:22b33429aff5 65 case 5:
sammir 0:22b33429aff5 66
sammir 0:22b33429aff5 67 myServo5.pulsewidth_us(pulseX);
sammir 0:22b33429aff5 68
sammir 0:22b33429aff5 69 break;
sammir 0:22b33429aff5 70
sammir 0:22b33429aff5 71 case 6: myServo6.pulsewidth_us(pulseX); break;
sammir 0:22b33429aff5 72
sammir 0:22b33429aff5 73 case 7:
sammir 0:22b33429aff5 74
sammir 0:22b33429aff5 75 myServo7.pulsewidth_us(pulseX);
sammir 0:22b33429aff5 76
sammir 0:22b33429aff5 77 break;
sammir 0:22b33429aff5 78
sammir 0:22b33429aff5 79 case 8:
sammir 0:22b33429aff5 80
sammir 0:22b33429aff5 81 myServo8.pulsewidth_us(pulseX);
sammir 0:22b33429aff5 82
sammir 0:22b33429aff5 83 break;
sammir 0:22b33429aff5 84 }
sammir 0:22b33429aff5 85
sammir 0:22b33429aff5 86
sammir 0:22b33429aff5 87
sammir 0:22b33429aff5 88 }
sammir 0:22b33429aff5 89
sammir 0:22b33429aff5 90 void init_servo()
sammir 0:22b33429aff5 91 {
sammir 0:22b33429aff5 92 myServo1.period_ms(20);
sammir 0:22b33429aff5 93 myServo2.period_ms(20);
sammir 0:22b33429aff5 94 myServo3.period_ms(20);
sammir 0:22b33429aff5 95 myServo4.period_ms(20);
sammir 0:22b33429aff5 96 myServo5.period_ms(20);
sammir 0:22b33429aff5 97 myServo6.period_ms(20);
sammir 0:22b33429aff5 98 myServo7.period_ms(20);
sammir 0:22b33429aff5 99 myServo8.period_ms(20);
sammir 0:22b33429aff5 100
sammir 0:22b33429aff5 101 }
sammir 0:22b33429aff5 102 void mover_ser2(uint8_t nmotor, uint8_t pos){
sammir 0:22b33429aff5 103 switch(nmotor){
sammir 0:22b33429aff5 104 case 1:
sammir 0:22b33429aff5 105 if(pos==1){
sammir 0:22b33429aff5 106 myServo1.pulsewidth_us(2650);
sammir 0:22b33429aff5 107 }
sammir 0:22b33429aff5 108 else if(pos==2){
sammir 0:22b33429aff5 109 myServo1.pulsewidth_us(1500);
sammir 0:22b33429aff5 110 }
sammir 0:22b33429aff5 111 else if(pos==3){
sammir 0:22b33429aff5 112 myServo2.pulsewidth_us(2650);
sammir 0:22b33429aff5 113 }
sammir 0:22b33429aff5 114 else if(pos==4){
sammir 0:22b33429aff5 115 myServo2.pulsewidth_us(1600);
sammir 0:22b33429aff5 116 }
sammir 0:22b33429aff5 117 else{
sammir 0:22b33429aff5 118 printf("comando fuera de las selecciones posibles");
sammir 0:22b33429aff5 119 }
sammir 0:22b33429aff5 120
sammir 0:22b33429aff5 121
sammir 0:22b33429aff5 122 break;
sammir 0:22b33429aff5 123
sammir 0:22b33429aff5 124 case 2:
sammir 0:22b33429aff5 125
sammir 0:22b33429aff5 126 if(pos==1){
sammir 0:22b33429aff5 127 myServo3.pulsewidth_us(2650);
sammir 0:22b33429aff5 128 }
sammir 0:22b33429aff5 129 else if(pos==2){
sammir 0:22b33429aff5 130 myServo3.pulsewidth_us(908);
sammir 0:22b33429aff5 131 }
sammir 0:22b33429aff5 132 else if(pos==3){
sammir 0:22b33429aff5 133 myServo4.pulsewidth_us(2650);
sammir 0:22b33429aff5 134 }
sammir 0:22b33429aff5 135 else if(pos==4){
sammir 0:22b33429aff5 136 myServo4.pulsewidth_us(1600);
sammir 0:22b33429aff5 137 }
sammir 0:22b33429aff5 138 else{
sammir 0:22b33429aff5 139 printf("comando fuera de las selecciones posibles");
sammir 0:22b33429aff5 140 }
sammir 0:22b33429aff5 141
sammir 0:22b33429aff5 142
sammir 0:22b33429aff5 143 break;
sammir 0:22b33429aff5 144
sammir 0:22b33429aff5 145 case 3:
sammir 0:22b33429aff5 146
sammir 0:22b33429aff5 147 if(pos==1){
sammir 0:22b33429aff5 148 myServo5.pulsewidth_us(2650);
sammir 0:22b33429aff5 149 }
sammir 0:22b33429aff5 150 else if(pos==2){
sammir 0:22b33429aff5 151 myServo5.pulsewidth_us(1200);
sammir 0:22b33429aff5 152 }
sammir 0:22b33429aff5 153 else if(pos==3){
sammir 0:22b33429aff5 154 myServo6.pulsewidth_us(908);
sammir 0:22b33429aff5 155 }
sammir 0:22b33429aff5 156 else if(pos==4){
sammir 0:22b33429aff5 157 myServo6.pulsewidth_us(1600);
sammir 0:22b33429aff5 158 }
sammir 0:22b33429aff5 159 else{
sammir 0:22b33429aff5 160 printf("comando fuera de las selecciones posibles");
sammir 0:22b33429aff5 161 }
sammir 0:22b33429aff5 162
sammir 0:22b33429aff5 163
sammir 0:22b33429aff5 164 break;
sammir 0:22b33429aff5 165
sammir 0:22b33429aff5 166 case 4:
sammir 0:22b33429aff5 167
sammir 0:22b33429aff5 168 if(pos==1){
sammir 0:22b33429aff5 169 myServo7.pulsewidth_us(2650);
sammir 0:22b33429aff5 170 }
sammir 0:22b33429aff5 171 else if(pos==2){
sammir 0:22b33429aff5 172 myServo7.pulsewidth_us(908);
sammir 0:22b33429aff5 173 }
sammir 0:22b33429aff5 174 else if(pos==3){
sammir 0:22b33429aff5 175 myServo8.pulsewidth_us(908);
sammir 0:22b33429aff5 176 }
sammir 0:22b33429aff5 177 else if(pos==4){
sammir 0:22b33429aff5 178 myServo8.pulsewidth_us(1800);
sammir 0:22b33429aff5 179 }
sammir 0:22b33429aff5 180 else{
sammir 0:22b33429aff5 181 printf("comando fuera de las selecciones posibles");
sammir 0:22b33429aff5 182 }
sammir 0:22b33429aff5 183
sammir 0:22b33429aff5 184
sammir 0:22b33429aff5 185 break;
sammir 0:22b33429aff5 186
sammir 0:22b33429aff5 187 }
sammir 0:22b33429aff5 188 }
sammir 0:22b33429aff5 189 void mover_ser3(){
sammir 0:22b33429aff5 190 myServo1.pulsewidth_us(600);
sammir 0:22b33429aff5 191 myServo3.pulsewidth_us(2500);
sammir 0:22b33429aff5 192 myServo5.pulsewidth_us(2500);
sammir 0:22b33429aff5 193 myServo7.pulsewidth_us(700);
sammir 0:22b33429aff5 194
sammir 0:22b33429aff5 195
sammir 0:22b33429aff5 196
sammir 0:22b33429aff5 197 myServo2.pulsewidth_us(1400);
sammir 0:22b33429aff5 198 myServo4.pulsewidth_us(2400);
sammir 0:22b33429aff5 199 myServo6.pulsewidth_us(1700);
sammir 0:22b33429aff5 200 myServo8.pulsewidth_us(300);
sammir 0:22b33429aff5 201 wait(1);
sammir 0:22b33429aff5 202 myServo1.pulsewidth_us(1500);
sammir 0:22b33429aff5 203 wait(1);
sammir 0:22b33429aff5 204 myServo2.pulsewidth_us(700);
sammir 0:22b33429aff5 205 wait(1);
sammir 0:22b33429aff5 206 myServo1.pulsewidth_us(600);
sammir 0:22b33429aff5 207 wait(1);
sammir 0:22b33429aff5 208 myServo5.pulsewidth_us(1600);
sammir 0:22b33429aff5 209 wait(1);
sammir 0:22b33429aff5 210 myServo6.pulsewidth_us(2400);
sammir 0:22b33429aff5 211 wait(1);
sammir 0:22b33429aff5 212 myServo5.pulsewidth_us(2500);
sammir 0:22b33429aff5 213
sammir 0:22b33429aff5 214 wait(1);
sammir 0:22b33429aff5 215 myServo7.pulsewidth_us(1700);
sammir 0:22b33429aff5 216 wait(1);
sammir 0:22b33429aff5 217 myServo8.pulsewidth_us(1400);
sammir 0:22b33429aff5 218 wait(1);
sammir 0:22b33429aff5 219 myServo7.pulsewidth_us(700);
sammir 0:22b33429aff5 220
sammir 0:22b33429aff5 221 wait(1);
sammir 0:22b33429aff5 222 myServo3.pulsewidth_us(1600);
sammir 0:22b33429aff5 223 wait(1);
sammir 0:22b33429aff5 224 myServo4.pulsewidth_us(1700);
sammir 0:22b33429aff5 225 wait(1);
sammir 0:22b33429aff5 226 myServo3.pulsewidth_us(2500);
sammir 0:22b33429aff5 227 wait(1);
sammir 0:22b33429aff5 228 myServo2.pulsewidth_us(1400);
sammir 0:22b33429aff5 229 myServo4.pulsewidth_us(2400);
sammir 0:22b33429aff5 230 myServo6.pulsewidth_us(1700);
sammir 0:22b33429aff5 231 myServo8.pulsewidth_us(300);
sammir 0:22b33429aff5 232
sammir 0:22b33429aff5 233 wait(1);
sammir 0:22b33429aff5 234
sammir 0:22b33429aff5 235
sammir 0:22b33429aff5 236
sammir 0:22b33429aff5 237
sammir 0:22b33429aff5 238 }
sammir 0:22b33429aff5 239
sammir 0:22b33429aff5 240
sammir 0:22b33429aff5 241
sammir 0:22b33429aff5 242
sammir 0:22b33429aff5 243
sammir 0:22b33429aff5 244
sammir 0:22b33429aff5 245
sammir 0:22b33429aff5 246
sammir 0:22b33429aff5 247
sammir 0:22b33429aff5 248
sammir 0:22b33429aff5 249
sammir 0:22b33429aff5 250
sammir 0:22b33429aff5 251