brayan paiba / Mbed 2 deprecated Nucleo_pwm

Piccolo, cuadrado, diagonal y circulo

Dependencies:   mbed

main.cpp@0:e0c1f3f89b27, 2017-09-16 (annotated)

Committer:
brayanpaiba
Date:
Sat Sep 16 14:22:04 2017 +0000
Revision:
0:e0c1f3f89b27
piccolo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
brayanpaiba 0:e0c1f3f89b27 1 #include "mbed.h"
brayanpaiba 0:e0c1f3f89b27 2
brayanpaiba 0:e0c1f3f89b27 3 PwmOut eje_x(PB_3);
brayanpaiba 0:e0c1f3f89b27 4 PwmOut eje_y(PB_4);
brayanpaiba 0:e0c1f3f89b27 5 PwmOut eje_z(PB_5);
brayanpaiba 0:e0c1f3f89b27 6
brayanpaiba 0:e0c1f3f89b27 7 double i=0,rad=0;
brayanpaiba 0:e0c1f3f89b27 8 double pulso_us=0,RET_PRU=0,gain=15,b=10;
brayanpaiba 0:e0c1f3f89b27 9 double grados=0,retardo_for=0.05,retardo=1,diam=70,var_x=0,var_y=0;
brayanpaiba 0:e0c1f3f89b27 10
brayanpaiba 0:e0c1f3f89b27 11
brayanpaiba 0:e0c1f3f89b27 12
brayanpaiba 0:e0c1f3f89b27 13 void REFERENCE(){
brayanpaiba 0:e0c1f3f89b27 14 eje_x.pulsewidth_us(1650);
brayanpaiba 0:e0c1f3f89b27 15 eje_y.pulsewidth_us(1635);
brayanpaiba 0:e0c1f3f89b27 16 eje_z.pulsewidth_us(750);
brayanpaiba 0:e0c1f3f89b27 17 wait_ms(500);
brayanpaiba 0:e0c1f3f89b27 18 }
brayanpaiba 0:e0c1f3f89b27 19
brayanpaiba 0:e0c1f3f89b27 20
brayanpaiba 0:e0c1f3f89b27 21 void SERVO_X(){
brayanpaiba 0:e0c1f3f89b27 22 pulso_us=((grados*1700)/(180))+790;
brayanpaiba 0:e0c1f3f89b27 23 eje_x.pulsewidth_us(pulso_us);
brayanpaiba 0:e0c1f3f89b27 24 wait_ms(0);
brayanpaiba 0:e0c1f3f89b27 25 }
brayanpaiba 0:e0c1f3f89b27 26
brayanpaiba 0:e0c1f3f89b27 27 void SERVO_Y(){
brayanpaiba 0:e0c1f3f89b27 28 pulso_us=((grados*1870)/(180))+700;
brayanpaiba 0:e0c1f3f89b27 29 eje_y.pulsewidth_us(pulso_us);
brayanpaiba 0:e0c1f3f89b27 30 wait_ms(0);
brayanpaiba 0:e0c1f3f89b27 31 }
brayanpaiba 0:e0c1f3f89b27 32
brayanpaiba 0:e0c1f3f89b27 33 void SERVO_Z(){
brayanpaiba 0:e0c1f3f89b27 34 pulso_us=((grados*1800)/(180))+650;
brayanpaiba 0:e0c1f3f89b27 35 eje_z.pulsewidth_us(pulso_us);
brayanpaiba 0:e0c1f3f89b27 36 wait_ms(100);
brayanpaiba 0:e0c1f3f89b27 37 }
brayanpaiba 0:e0c1f3f89b27 38
brayanpaiba 0:e0c1f3f89b27 39 void PruevaX(){
brayanpaiba 0:e0c1f3f89b27 40 grados=0;
brayanpaiba 0:e0c1f3f89b27 41 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 42 wait_ms(RET_PRU);
brayanpaiba 0:e0c1f3f89b27 43 for(i=0;i<=90;i=i+10){
brayanpaiba 0:e0c1f3f89b27 44 grados=i;
brayanpaiba 0:e0c1f3f89b27 45 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 46 wait_ms(retardo);
brayanpaiba 0:e0c1f3f89b27 47 }
brayanpaiba 0:e0c1f3f89b27 48 wait_ms(RET_PRU);
brayanpaiba 0:e0c1f3f89b27 49 grados=180;
brayanpaiba 0:e0c1f3f89b27 50 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 51 wait_ms(RET_PRU);
brayanpaiba 0:e0c1f3f89b27 52 }
brayanpaiba 0:e0c1f3f89b27 53 void PruevaY(){
brayanpaiba 0:e0c1f3f89b27 54 grados=0;
brayanpaiba 0:e0c1f3f89b27 55 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 56 wait_ms(RET_PRU);
brayanpaiba 0:e0c1f3f89b27 57 for(i=0;i<=90;i=i+10){
brayanpaiba 0:e0c1f3f89b27 58 grados=i;
brayanpaiba 0:e0c1f3f89b27 59 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 60 wait_ms(retardo);
brayanpaiba 0:e0c1f3f89b27 61 }
brayanpaiba 0:e0c1f3f89b27 62 wait_ms(RET_PRU);
brayanpaiba 0:e0c1f3f89b27 63 grados=180;
brayanpaiba 0:e0c1f3f89b27 64 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 65 wait_ms(RET_PRU);
brayanpaiba 0:e0c1f3f89b27 66 }
brayanpaiba 0:e0c1f3f89b27 67
brayanpaiba 0:e0c1f3f89b27 68 void ESCRIBIR(){
brayanpaiba 0:e0c1f3f89b27 69 grados=120;
brayanpaiba 0:e0c1f3f89b27 70 SERVO_Z();
brayanpaiba 0:e0c1f3f89b27 71 }
brayanpaiba 0:e0c1f3f89b27 72
brayanpaiba 0:e0c1f3f89b27 73 void ESCRIBIR_0(){
brayanpaiba 0:e0c1f3f89b27 74 grados=30;
brayanpaiba 0:e0c1f3f89b27 75 SERVO_Z();
brayanpaiba 0:e0c1f3f89b27 76 }
brayanpaiba 0:e0c1f3f89b27 77
brayanpaiba 0:e0c1f3f89b27 78 void LINEA_DIAG(){
brayanpaiba 0:e0c1f3f89b27 79 REFERENCE();
brayanpaiba 0:e0c1f3f89b27 80 wait(2);
brayanpaiba 0:e0c1f3f89b27 81 for(i=0;i<=1800;i=i+5){
brayanpaiba 0:e0c1f3f89b27 82 grados=i;
brayanpaiba 0:e0c1f3f89b27 83 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 84 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 85 wait_ms(retardo);
brayanpaiba 0:e0c1f3f89b27 86 ESCRIBIR();
brayanpaiba 0:e0c1f3f89b27 87 }
brayanpaiba 0:e0c1f3f89b27 88 ESCRIBIR_0();
brayanpaiba 0:e0c1f3f89b27 89 }
brayanpaiba 0:e0c1f3f89b27 90
brayanpaiba 0:e0c1f3f89b27 91 void CUADRADO(){
brayanpaiba 0:e0c1f3f89b27 92 REFERENCE();
brayanpaiba 0:e0c1f3f89b27 93 grados=0;
brayanpaiba 0:e0c1f3f89b27 94 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 95 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 96 wait(2);
brayanpaiba 0:e0c1f3f89b27 97 for(i=0;i<=180;i=i+5){
brayanpaiba 0:e0c1f3f89b27 98 grados=i;
brayanpaiba 0:e0c1f3f89b27 99 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 100 wait_ms(retardo);
brayanpaiba 0:e0c1f3f89b27 101 ESCRIBIR();
brayanpaiba 0:e0c1f3f89b27 102 }
brayanpaiba 0:e0c1f3f89b27 103 for(i=0;i<=180;i=i+5){
brayanpaiba 0:e0c1f3f89b27 104 grados=i;
brayanpaiba 0:e0c1f3f89b27 105 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 106 wait_ms(retardo);
brayanpaiba 0:e0c1f3f89b27 107 ESCRIBIR();
brayanpaiba 0:e0c1f3f89b27 108 }
brayanpaiba 0:e0c1f3f89b27 109 for(i=180;i>=0;i=i-5){
brayanpaiba 0:e0c1f3f89b27 110 grados=i;
brayanpaiba 0:e0c1f3f89b27 111 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 112 wait_ms(retardo);
brayanpaiba 0:e0c1f3f89b27 113 ESCRIBIR();
brayanpaiba 0:e0c1f3f89b27 114 }
brayanpaiba 0:e0c1f3f89b27 115 for(i=180;i>=0;i=i-5){
brayanpaiba 0:e0c1f3f89b27 116 grados=i;
brayanpaiba 0:e0c1f3f89b27 117 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 118 wait_ms(retardo);
brayanpaiba 0:e0c1f3f89b27 119 ESCRIBIR();
brayanpaiba 0:e0c1f3f89b27 120 }
brayanpaiba 0:e0c1f3f89b27 121 for(i=180;i<=0;i=i+10){
brayanpaiba 0:e0c1f3f89b27 122 grados=i;
brayanpaiba 0:e0c1f3f89b27 123 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 124 wait_ms(retardo);
brayanpaiba 0:e0c1f3f89b27 125 ESCRIBIR();
brayanpaiba 0:e0c1f3f89b27 126 }
brayanpaiba 0:e0c1f3f89b27 127 ESCRIBIR_0();
brayanpaiba 0:e0c1f3f89b27 128 }
brayanpaiba 0:e0c1f3f89b27 129 void CIRCULO(){
brayanpaiba 0:e0c1f3f89b27 130 REFERENCE();
brayanpaiba 0:e0c1f3f89b27 131 wait(2);
brayanpaiba 0:e0c1f3f89b27 132 for(i=0;i<360;i=i+10){
brayanpaiba 0:e0c1f3f89b27 133 rad=(i*2*3.1416)/(180);
brayanpaiba 0:e0c1f3f89b27 134 grados=(((cos(rad)+1)*180/2)*140/180)+20;
brayanpaiba 0:e0c1f3f89b27 135 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 136 grados=(((sin(rad)+1)*180/2)*140/180)+20;
brayanpaiba 0:e0c1f3f89b27 137 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 138 wait_ms(retardo);
brayanpaiba 0:e0c1f3f89b27 139 ESCRIBIR();
brayanpaiba 0:e0c1f3f89b27 140 }
brayanpaiba 0:e0c1f3f89b27 141 ESCRIBIR_0();
brayanpaiba 0:e0c1f3f89b27 142 }
brayanpaiba 0:e0c1f3f89b27 143
brayanpaiba 0:e0c1f3f89b27 144 void DIBUJO(){
brayanpaiba 0:e0c1f3f89b27 145 REFERENCE();
brayanpaiba 0:e0c1f3f89b27 146 wait(2);
brayanpaiba 0:e0c1f3f89b27 147 grados=0+gain;
brayanpaiba 0:e0c1f3f89b27 148 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 149 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 150 ESCRIBIR();
brayanpaiba 0:e0c1f3f89b27 151 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 152
brayanpaiba 0:e0c1f3f89b27 153 grados=30+gain;
brayanpaiba 0:e0c1f3f89b27 154 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 155 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 156
brayanpaiba 0:e0c1f3f89b27 157 grados=60+gain;
brayanpaiba 0:e0c1f3f89b27 158 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 159 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 160
brayanpaiba 0:e0c1f3f89b27 161 grados=90+gain;
brayanpaiba 0:e0c1f3f89b27 162 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 163 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 164
brayanpaiba 0:e0c1f3f89b27 165 grados=120+gain;
brayanpaiba 0:e0c1f3f89b27 166 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 167 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 168
brayanpaiba 0:e0c1f3f89b27 169 grados=150+gain;
brayanpaiba 0:e0c1f3f89b27 170 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 171 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 172
brayanpaiba 0:e0c1f3f89b27 173 grados=150+gain;
brayanpaiba 0:e0c1f3f89b27 174 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 175 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 176
brayanpaiba 0:e0c1f3f89b27 177 grados=120+gain-b;
brayanpaiba 0:e0c1f3f89b27 178 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 179 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 180
brayanpaiba 0:e0c1f3f89b27 181 grados=90+gain-b;
brayanpaiba 0:e0c1f3f89b27 182 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 183 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 184
brayanpaiba 0:e0c1f3f89b27 185 grados=60+gain-b;
brayanpaiba 0:e0c1f3f89b27 186 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 187 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 188
brayanpaiba 0:e0c1f3f89b27 189 grados=30+gain-b;
brayanpaiba 0:e0c1f3f89b27 190 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 191 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 192
brayanpaiba 0:e0c1f3f89b27 193 grados=0+gain-b;
brayanpaiba 0:e0c1f3f89b27 194 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 195 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 196
brayanpaiba 0:e0c1f3f89b27 197 grados=0+gain-b;
brayanpaiba 0:e0c1f3f89b27 198 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 199 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 200
brayanpaiba 0:e0c1f3f89b27 201 ESCRIBIR_0();
brayanpaiba 0:e0c1f3f89b27 202
brayanpaiba 0:e0c1f3f89b27 203 grados=150+gain;
brayanpaiba 0:e0c1f3f89b27 204 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 205 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 206
brayanpaiba 0:e0c1f3f89b27 207 grados=0+gain;
brayanpaiba 0:e0c1f3f89b27 208 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 209 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 210
brayanpaiba 0:e0c1f3f89b27 211 ESCRIBIR();
brayanpaiba 0:e0c1f3f89b27 212 for(i=0;i<45;i++){
brayanpaiba 0:e0c1f3f89b27 213 grados=i+gain;
brayanpaiba 0:e0c1f3f89b27 214 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 215 grados=150-i+gain;
brayanpaiba 0:e0c1f3f89b27 216 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 217 wait(retardo_for);
brayanpaiba 0:e0c1f3f89b27 218 }
brayanpaiba 0:e0c1f3f89b27 219
brayanpaiba 0:e0c1f3f89b27 220 ESCRIBIR_0();
brayanpaiba 0:e0c1f3f89b27 221
brayanpaiba 0:e0c1f3f89b27 222 grados=150+gain;
brayanpaiba 0:e0c1f3f89b27 223 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 224 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 225
brayanpaiba 0:e0c1f3f89b27 226 grados=0+gain;
brayanpaiba 0:e0c1f3f89b27 227 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 228 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 229
brayanpaiba 0:e0c1f3f89b27 230 ESCRIBIR();
brayanpaiba 0:e0c1f3f89b27 231 for(i=0;i<45;i++){
brayanpaiba 0:e0c1f3f89b27 232 grados=150-i+gain;
brayanpaiba 0:e0c1f3f89b27 233 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 234 grados=i+gain;
brayanpaiba 0:e0c1f3f89b27 235 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 236 wait(retardo_for);
brayanpaiba 0:e0c1f3f89b27 237 }
brayanpaiba 0:e0c1f3f89b27 238
brayanpaiba 0:e0c1f3f89b27 239 ESCRIBIR_0();
brayanpaiba 0:e0c1f3f89b27 240
brayanpaiba 0:e0c1f3f89b27 241 grados=150+gain;
brayanpaiba 0:e0c1f3f89b27 242 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 243 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 244
brayanpaiba 0:e0c1f3f89b27 245 grados=60+gain;
brayanpaiba 0:e0c1f3f89b27 246 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 247 wait(retardo);
brayanpaiba 0:e0c1f3f89b27 248
brayanpaiba 0:e0c1f3f89b27 249 for(i=180;i<360;i=i+5){
brayanpaiba 0:e0c1f3f89b27 250 rad=(i*3.1416)/(180);
brayanpaiba 0:e0c1f3f89b27 251 var_x=((cos(rad)+1)*diam/2)*1.0;
brayanpaiba 0:e0c1f3f89b27 252 grados=var_x+30+gain;
brayanpaiba 0:e0c1f3f89b27 253 SERVO_X();
brayanpaiba 0:e0c1f3f89b27 254 var_y=((sin(rad)+1)*diam/2)*1.5;
brayanpaiba 0:e0c1f3f89b27 255 grados=var_y+120+gain;
brayanpaiba 0:e0c1f3f89b27 256 SERVO_Y();
brayanpaiba 0:e0c1f3f89b27 257 ESCRIBIR();
brayanpaiba 0:e0c1f3f89b27 258 }
brayanpaiba 0:e0c1f3f89b27 259 ESCRIBIR_0();
brayanpaiba 0:e0c1f3f89b27 260 }
brayanpaiba 0:e0c1f3f89b27 261
brayanpaiba 0:e0c1f3f89b27 262
brayanpaiba 0:e0c1f3f89b27 263 int main() {
brayanpaiba 0:e0c1f3f89b27 264 eje_x.period_us(20000);
brayanpaiba 0:e0c1f3f89b27 265 eje_y.period_us(20000);
brayanpaiba 0:e0c1f3f89b27 266 eje_z.period_us(20000);
brayanpaiba 0:e0c1f3f89b27 267 REFERENCE();
brayanpaiba 0:e0c1f3f89b27 268
brayanpaiba 0:e0c1f3f89b27 269 while(1) {
brayanpaiba 0:e0c1f3f89b27 270
brayanpaiba 0:e0c1f3f89b27 271 DIBUJO();
brayanpaiba 0:e0c1f3f89b27 272
brayanpaiba 0:e0c1f3f89b27 273
brayanpaiba 0:e0c1f3f89b27 274
brayanpaiba 0:e0c1f3f89b27 275 }
brayanpaiba 0:e0c1f3f89b27 276 }