Hexapod / Mbed OS PwmOut_HelloWorld

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:   Servo SoftPWM

Fork of PwmOut_HelloWorld by Francisco Núñez Cid

hexapod.cpp@9:8e25ad440b2e, 2018-10-09 (annotated)

Committer:
Francisco_12
Date:
Tue Oct 09 18:20:43 2018 +0000
Revision:
9:8e25ad440b2e
Parent:
main.cpp@8:b0a6c16e781b
Child:
10:88d5a9711af8
Creaci?n de hexapod.h

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Francisco_12 9:8e25ad440b2e 1 #include "hexapod.h"
Francisco_12 7:4bc4af0236f0 2
Francisco_12 7:4bc4af0236f0 3 Serial my_serial(PC_6, PA_12); //Puerto serie
Francisco_12 4:107e8818ba81 4
Francisco_12 4:107e8818ba81 5 /*SoftPWM servo_C_B_R(PB_8); //Servo Coxa_Behind_Right
Francisco_12 4:107e8818ba81 6 SoftPWM servo_F_B_R(PB_9); //Servo Femur_Behind_Right
Francisco_12 4:107e8818ba81 7 SoftPWM servo_T_B_R(PB_7); //Servo Tibia_Behind_Right
Francisco_12 4:107e8818ba81 8
Francisco_12 4:107e8818ba81 9 SoftPWM servo_C_M_R(PA_0); //Servo Coxa_Medium_Right
Francisco_12 4:107e8818ba81 10 SoftPWM servo_F_M_R(PA_1); //Servo Femur_Medium_Right
Francisco_12 4:107e8818ba81 11 SoftPWM servo_T_M_R(PB_0); //Servo Tibia_Medium_Right
Francisco_12 4:107e8818ba81 12
Francisco_12 7:4bc4af0236f0 13 SoftPWM servo_C_F_R(PB_1); //Servo Coxa_Forward_Right
Francisco_12 7:4bc4af0236f0 14 SoftPWM servo_F_F_R(PA_3); //Servo Femur_Forward_Right
Francisco_12 7:4bc4af0236f0 15 SoftPWM servo_T_F_R(PA_2); //Servo Tibia_Forward_Right
mbedAustin 1:5160ea45399b 16
Francisco_12 4:107e8818ba81 17 SoftPWM servo_C_B_L(PA_5); //Servo Coxa_Behind_Left
Francisco_12 4:107e8818ba81 18 SoftPWM servo_F_B_L(PA_6); //Servo Femur_Behind_Left
Francisco_12 4:107e8818ba81 19 SoftPWM servo_T_B_L(PA_7); //Servo Tibia_Behind_Left
Francisco_12 4:107e8818ba81 20
Francisco_12 4:107e8818ba81 21 SoftPWM servo_C_M_L(PB_6); //Servo Coxa_Medium_Left
Francisco_12 4:107e8818ba81 22 SoftPWM servo_F_M_L(PA_9); //Servo Femur_Medium_Left
Francisco_12 4:107e8818ba81 23 SoftPWM servo_T_M_L(PA_8); //Servo Tibia_Medium_Left
mbedAustin 1:5160ea45399b 24
Francisco_12 7:4bc4af0236f0 25 SoftPWM servo_C_F_L(PB_10); //Servo Coxa_Forward_Left
Francisco_12 7:4bc4af0236f0 26 SoftPWM servo_F_F_L(PB_3); //Servo Femur_Forward_Left
Francisco_12 7:4bc4af0236f0 27 SoftPWM servo_T_F_L(PA_10); //Servo Tibia_Forward_Left
Francisco_12 4:107e8818ba81 28 */
Francisco_12 4:107e8818ba81 29
Francisco_12 7:4bc4af0236f0 30 SoftPWM pata1[] = {SoftPWM(PB_8),SoftPWM(PB_9),SoftPWM(PB_7)}; //{Coxa, Femur, Tibia} -> Behind Right
Francisco_12 8:b0a6c16e781b 31 SoftPWM pata2[] = {SoftPWM(PA_0),SoftPWM(PA_1),SoftPWM(PB_0)}; //{Coxa, Femur, Tibia} -> Medium Right
Francisco_12 8:b0a6c16e781b 32 SoftPWM pata3[] = {SoftPWM(PB_1),SoftPWM(PA_3),SoftPWM(PA_2)}; //{Coxa, Femur, Tibia} -> Forward Right
Francisco_12 8:b0a6c16e781b 33 SoftPWM pata4[] = {SoftPWM(PA_5),SoftPWM(PA_6),SoftPWM(PA_7)}; //{Coxa, Femur, Tibia} -> Behind Left
Francisco_12 8:b0a6c16e781b 34 SoftPWM pata5[] = {SoftPWM(PB_6),SoftPWM(PA_9),SoftPWM(PA_8)}; //{Coxa, Femur, Tibia} -> Medium Left
Francisco_12 8:b0a6c16e781b 35 SoftPWM pata6[] = {SoftPWM(PB_10),SoftPWM(PB_3),SoftPWM(PA_10)}; //{Coxa, Femur, Tibia} -> Forward Left
Francisco_12 4:107e8818ba81 36
mbed_official 0:50d2b9c62765 37 int main() {
Francisco_12 4:107e8818ba81 38 pata3[1].period_us(20000); //20ms period, typical for analog RC servo
Francisco_12 4:107e8818ba81 39 bool mov = false;
Francisco_12 4:107e8818ba81 40 while(1){
Francisco_12 6:69a6ff2fd76d 41 if (mov == true) {
Francisco_12 5:3699580da2e2 42 mueve_servo(&pata3[1], AVANZA);
Francisco_12 4:107e8818ba81 43 } else {
Francisco_12 5:3699580da2e2 44 mueve_servo(&pata3[1], HOME);
Francisco_12 6:69a6ff2fd76d 45 }
Francisco_12 6:69a6ff2fd76d 46 //mueve_pata('A');
Francisco_12 4:107e8818ba81 47 mov = !mov;
Francisco_12 4:107e8818ba81 48 wait_ms(1000); //Tiempo de espera entre movimiento y movimiento -> 250
Francisco_12 4:107e8818ba81 49 }
Francisco_12 4:107e8818ba81 50 }
Francisco_12 4:107e8818ba81 51
Francisco_12 8:b0a6c16e781b 52 void mueve_servo(SoftPWM* pata, int grados){ //Función que realiza el movimiento de un servo
Francisco_12 4:107e8818ba81 53 int th=0;
Francisco_12 6:69a6ff2fd76d 54 th = 560 + (1925*grados)/180; //Límites del Servo [560 - 2485] -> OFFSET:1925
Francisco_12 4:107e8818ba81 55 pata->pulsewidth_us(th);
Francisco_12 4:107e8818ba81 56 }
Francisco_12 4:107e8818ba81 57
Francisco_12 8:b0a6c16e781b 58 void mueve_pata1(char pos){ //Función que realiza el movimiento de una pata dependiendo de lo que se quiera hacer
Francisco_12 4:107e8818ba81 59 switch(pos){
Francisco_12 4:107e8818ba81 60 case 'A': //Avanza
Francisco_12 7:4bc4af0236f0 61 mueve_servo(&pata1[0], MOV_AV);
Francisco_12 7:4bc4af0236f0 62 mueve_servo(&pata1[1], MOV_AV);
Francisco_12 7:4bc4af0236f0 63 mueve_servo(&pata1[2], MOV_AV);
Francisco_12 7:4bc4af0236f0 64 break;
Francisco_12 7:4bc4af0236f0 65 case 'R': //Retrocede
Francisco_12 7:4bc4af0236f0 66 mueve_servo(&pata1[0], MOV_RE);
Francisco_12 7:4bc4af0236f0 67 mueve_servo(&pata1[1], MOV_RE);
Francisco_12 7:4bc4af0236f0 68 mueve_servo(&pata1[2], MOV_RE);
Francisco_12 7:4bc4af0236f0 69 break;
Francisco_12 7:4bc4af0236f0 70 case 'H': //Home
Francisco_12 7:4bc4af0236f0 71 mueve_servo(&pata1[0], INICIO);
Francisco_12 7:4bc4af0236f0 72 mueve_servo(&pata1[1], INICIO);
Francisco_12 7:4bc4af0236f0 73 mueve_servo(&pata1[2], INICIO);
Francisco_12 7:4bc4af0236f0 74 break;
Francisco_12 7:4bc4af0236f0 75 case 'S': //Sube
Francisco_12 7:4bc4af0236f0 76 mueve_servo(&pata1[0], INICIO);
Francisco_12 7:4bc4af0236f0 77 mueve_servo(&pata1[1], INICIO);
Francisco_12 7:4bc4af0236f0 78 mueve_servo(&pata1[2], INICIO);
Francisco_12 7:4bc4af0236f0 79 break;
Francisco_12 7:4bc4af0236f0 80 default: //Home
Francisco_12 7:4bc4af0236f0 81 mueve_servo(&pata1[0], INICIO);
Francisco_12 7:4bc4af0236f0 82 mueve_servo(&pata1[1], INICIO);
Francisco_12 7:4bc4af0236f0 83 mueve_servo(&pata1[2], INICIO);
Francisco_12 7:4bc4af0236f0 84 break;
Francisco_12 7:4bc4af0236f0 85 }
Francisco_12 7:4bc4af0236f0 86 }
Francisco_12 7:4bc4af0236f0 87
Francisco_12 7:4bc4af0236f0 88 void mueve_pata2(char pos){
Francisco_12 7:4bc4af0236f0 89 switch(pos){
Francisco_12 7:4bc4af0236f0 90 case 'A': //Avanza
Francisco_12 7:4bc4af0236f0 91 mueve_servo(&pata2[0], MOV_AV);
Francisco_12 7:4bc4af0236f0 92 mueve_servo(&pata2[1], MOV_AV);
Francisco_12 7:4bc4af0236f0 93 mueve_servo(&pata2[2], MOV_AV);
Francisco_12 7:4bc4af0236f0 94 break;
Francisco_12 7:4bc4af0236f0 95 case 'R': //Retrocede
Francisco_12 7:4bc4af0236f0 96 mueve_servo(&pata2[0], MOV_RE);
Francisco_12 7:4bc4af0236f0 97 mueve_servo(&pata2[1], MOV_RE);
Francisco_12 7:4bc4af0236f0 98 mueve_servo(&pata2[2], MOV_RE);
Francisco_12 7:4bc4af0236f0 99 break;
Francisco_12 7:4bc4af0236f0 100 case 'H': //Home
Francisco_12 7:4bc4af0236f0 101 mueve_servo(&pata2[0], INICIO);
Francisco_12 7:4bc4af0236f0 102 mueve_servo(&pata2[1], INICIO);
Francisco_12 7:4bc4af0236f0 103 mueve_servo(&pata2[2], INICIO);
Francisco_12 7:4bc4af0236f0 104 break;
Francisco_12 7:4bc4af0236f0 105 default: //Home
Francisco_12 7:4bc4af0236f0 106 mueve_servo(&pata2[0], INICIO);
Francisco_12 7:4bc4af0236f0 107 mueve_servo(&pata2[1], INICIO);
Francisco_12 7:4bc4af0236f0 108 mueve_servo(&pata2[2], INICIO);
Francisco_12 7:4bc4af0236f0 109 break;
Francisco_12 7:4bc4af0236f0 110 }
Francisco_12 7:4bc4af0236f0 111 }
Francisco_12 7:4bc4af0236f0 112
Francisco_12 7:4bc4af0236f0 113 void mueve_pata3(char pos){
Francisco_12 7:4bc4af0236f0 114 switch(pos){
Francisco_12 7:4bc4af0236f0 115 case 'A': //Avanza
Francisco_12 7:4bc4af0236f0 116 mueve_servo(&pata3[0], MOV_AV);
Francisco_12 7:4bc4af0236f0 117 mueve_servo(&pata3[1], MOV_AV);
Francisco_12 7:4bc4af0236f0 118 mueve_servo(&pata3[2], MOV_AV);
Francisco_12 4:107e8818ba81 119 break;
Francisco_12 4:107e8818ba81 120 case 'R': //Retrocede
Francisco_12 7:4bc4af0236f0 121 mueve_servo(&pata3[0], MOV_RE);
Francisco_12 7:4bc4af0236f0 122 mueve_servo(&pata3[1], MOV_RE);
Francisco_12 7:4bc4af0236f0 123 mueve_servo(&pata3[2], MOV_RE);
Francisco_12 7:4bc4af0236f0 124 break;
Francisco_12 7:4bc4af0236f0 125 case 'H': //Home
Francisco_12 7:4bc4af0236f0 126 mueve_servo(&pata3[0], INICIO);
Francisco_12 7:4bc4af0236f0 127 mueve_servo(&pata3[1], INICIO);
Francisco_12 7:4bc4af0236f0 128 mueve_servo(&pata3[2], INICIO);
Francisco_12 7:4bc4af0236f0 129 break;
Francisco_12 7:4bc4af0236f0 130 default: //Home
Francisco_12 7:4bc4af0236f0 131 mueve_servo(&pata3[0], INICIO);
Francisco_12 7:4bc4af0236f0 132 mueve_servo(&pata3[1], INICIO);
Francisco_12 7:4bc4af0236f0 133 mueve_servo(&pata3[2], INICIO);
Francisco_12 7:4bc4af0236f0 134 break;
Francisco_12 7:4bc4af0236f0 135 }
Francisco_12 7:4bc4af0236f0 136 }
Francisco_12 7:4bc4af0236f0 137
Francisco_12 7:4bc4af0236f0 138 void mueve_pata4(char pos){
Francisco_12 7:4bc4af0236f0 139 switch(pos){
Francisco_12 7:4bc4af0236f0 140 case 'A': //Avanza
Francisco_12 7:4bc4af0236f0 141 mueve_servo(&pata4[0], MOV_AV);
Francisco_12 7:4bc4af0236f0 142 mueve_servo(&pata4[1], MOV_AV);
Francisco_12 7:4bc4af0236f0 143 mueve_servo(&pata4[2], MOV_AV);
Francisco_12 7:4bc4af0236f0 144 break;
Francisco_12 7:4bc4af0236f0 145 case 'R': //Retrocede
Francisco_12 7:4bc4af0236f0 146 mueve_servo(&pata4[0], MOV_RE);
Francisco_12 7:4bc4af0236f0 147 mueve_servo(&pata4[1], MOV_RE);
Francisco_12 7:4bc4af0236f0 148 mueve_servo(&pata4[2], MOV_RE);
Francisco_12 7:4bc4af0236f0 149 break;
Francisco_12 7:4bc4af0236f0 150 case 'H': //Home
Francisco_12 7:4bc4af0236f0 151 mueve_servo(&pata4[0], INICIO);
Francisco_12 7:4bc4af0236f0 152 mueve_servo(&pata4[1], INICIO);
Francisco_12 7:4bc4af0236f0 153 mueve_servo(&pata4[2], INICIO);
Francisco_12 7:4bc4af0236f0 154 break;
Francisco_12 7:4bc4af0236f0 155 default: //Home
Francisco_12 7:4bc4af0236f0 156 mueve_servo(&pata4[0], INICIO);
Francisco_12 7:4bc4af0236f0 157 mueve_servo(&pata4[1], INICIO);
Francisco_12 7:4bc4af0236f0 158 mueve_servo(&pata4[2], INICIO);
Francisco_12 7:4bc4af0236f0 159 break;
Francisco_12 7:4bc4af0236f0 160 }
Francisco_12 7:4bc4af0236f0 161 }
Francisco_12 7:4bc4af0236f0 162
Francisco_12 7:4bc4af0236f0 163 void mueve_pata5(char pos){
Francisco_12 7:4bc4af0236f0 164 switch(pos){
Francisco_12 7:4bc4af0236f0 165 case 'A': //Avanza
Francisco_12 7:4bc4af0236f0 166 mueve_servo(&pata5[0], MOV_AV);
Francisco_12 7:4bc4af0236f0 167 mueve_servo(&pata5[1], MOV_AV);
Francisco_12 7:4bc4af0236f0 168 mueve_servo(&pata5[2], MOV_AV);
Francisco_12 7:4bc4af0236f0 169 break;
Francisco_12 7:4bc4af0236f0 170 case 'R': //Retrocede
Francisco_12 7:4bc4af0236f0 171 mueve_servo(&pata5[0], MOV_RE);
Francisco_12 7:4bc4af0236f0 172 mueve_servo(&pata5[1], MOV_RE);
Francisco_12 7:4bc4af0236f0 173 mueve_servo(&pata5[2], MOV_RE);
Francisco_12 4:107e8818ba81 174 break;
Francisco_12 4:107e8818ba81 175 case 'H': //Home
Francisco_12 7:4bc4af0236f0 176 mueve_servo(&pata5[0], INICIO);
Francisco_12 7:4bc4af0236f0 177 mueve_servo(&pata5[1], INICIO);
Francisco_12 7:4bc4af0236f0 178 mueve_servo(&pata5[2], INICIO);
Francisco_12 4:107e8818ba81 179 break;
Francisco_12 4:107e8818ba81 180 default: //Home
Francisco_12 7:4bc4af0236f0 181 mueve_servo(&pata5[0], INICIO);
Francisco_12 7:4bc4af0236f0 182 mueve_servo(&pata5[1], INICIO);
Francisco_12 7:4bc4af0236f0 183 mueve_servo(&pata5[2], INICIO);
Francisco_12 4:107e8818ba81 184 break;
Francisco_12 4:107e8818ba81 185 }
Francisco_12 7:4bc4af0236f0 186 }
Francisco_12 7:4bc4af0236f0 187
Francisco_12 7:4bc4af0236f0 188 void mueve_pata6(char pos){
Francisco_12 7:4bc4af0236f0 189 switch(pos){
Francisco_12 7:4bc4af0236f0 190 case 'A': //Avanza
Francisco_12 7:4bc4af0236f0 191 mueve_servo(&pata6[0], MOV_AV);
Francisco_12 7:4bc4af0236f0 192 mueve_servo(&pata6[1], MOV_AV);
Francisco_12 7:4bc4af0236f0 193 mueve_servo(&pata6[2], MOV_AV);
Francisco_12 7:4bc4af0236f0 194 break;
Francisco_12 7:4bc4af0236f0 195 case 'R': //Retrocede
Francisco_12 7:4bc4af0236f0 196 mueve_servo(&pata6[0], MOV_RE);
Francisco_12 7:4bc4af0236f0 197 mueve_servo(&pata6[1], MOV_RE);
Francisco_12 7:4bc4af0236f0 198 mueve_servo(&pata6[2], MOV_RE);
Francisco_12 7:4bc4af0236f0 199 break;
Francisco_12 7:4bc4af0236f0 200 case 'H': //Home
Francisco_12 7:4bc4af0236f0 201 mueve_servo(&pata6[0], INICIO);
Francisco_12 7:4bc4af0236f0 202 mueve_servo(&pata6[1], INICIO);
Francisco_12 7:4bc4af0236f0 203 mueve_servo(&pata6[2], INICIO);
Francisco_12 7:4bc4af0236f0 204 break;
Francisco_12 7:4bc4af0236f0 205 default: //Home
Francisco_12 7:4bc4af0236f0 206 mueve_servo(&pata6[0], INICIO);
Francisco_12 7:4bc4af0236f0 207 mueve_servo(&pata6[1], INICIO);
Francisco_12 7:4bc4af0236f0 208 mueve_servo(&pata6[2], INICIO);
Francisco_12 7:4bc4af0236f0 209 break;
Francisco_12 7:4bc4af0236f0 210 }
Francisco_12 7:4bc4af0236f0 211 }
Francisco_12 7:4bc4af0236f0 212
Francisco_12 8:b0a6c16e781b 213 void hexapod_mov(int type){ //Función que hace el movimiento del hexápodo dependiendo de lo que se quiera hacer
Francisco_12 7:4bc4af0236f0 214 if(type == ARRIBA){
Francisco_12 8:b0a6c16e781b 215 //Rellenar el movimiento con las patas
Francisco_12 7:4bc4af0236f0 216 }else if(type == ABAJO){
Francisco_12 8:b0a6c16e781b 217 //Rellenar el movimiento con las patas
Francisco_12 7:4bc4af0236f0 218 }else if(type == HOME){
Francisco_12 8:b0a6c16e781b 219 //Rellenar el movimiento con las patas
Francisco_12 7:4bc4af0236f0 220 }else if(type == AVANZA){
Francisco_12 8:b0a6c16e781b 221 //Rellenar el movimiento con las patas
Francisco_12 7:4bc4af0236f0 222 }else if(type == RETROCEDE){
Francisco_12 8:b0a6c16e781b 223 //Rellenar el movimiento con las patas
Francisco_12 7:4bc4af0236f0 224 }else if(type == DERECHA){
Francisco_12 8:b0a6c16e781b 225 //Rellenar el movimiento con las patas
Francisco_12 7:4bc4af0236f0 226 }else if(type == IZQUIERDA){
Francisco_12 8:b0a6c16e781b 227 //Rellenar el movimiento con las patas
Francisco_12 7:4bc4af0236f0 228 }
mbed_official 0:50d2b9c62765 229 }