![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ライブラリ化を行った後
Dependencies: QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo
Fork of 17robo_Practice1 by
Diff: main.cpp
- Revision:
- 48:96b5f5ebdfb0
- Parent:
- 47:6ea046767494
- Child:
- 49:b041c815c063
--- a/main.cpp Tue Sep 12 10:36:31 2017 +0000 +++ b/main.cpp Wed Sep 13 00:19:44 2017 +0000 @@ -14,7 +14,6 @@ #include "cyclic_IO.h" #include "cylinder.h" - #define pc_baud 460800 #define sbdbt_tx p13 #define sbdbt_rx p14 @@ -59,9 +58,9 @@ //#define pin_interrupt_reload p30 // DigitalOut led1(LED1); -//DigitalOut led2(LED2); -//DigitalOut led3(LED3); -//DigitalOut led4(LED4); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min); Serial pc(USBTX,USBRX); RS422 rs422(rs422_tx, rs422_rx); @@ -129,10 +128,10 @@ { setup(); while(1) { -// led1 = interrupt_sholderleft_max; -// led2 = interrupt_sholderleft_min; -// led3 = interrupt_sholderright_max; -// led4 = interrupt_sholderright_min; + led1 = interrupt_sholderleft_max; + led2 = interrupt_sholderleft_min; + led3 = interrupt_sholderright_max; + led4 = interrupt_sholderright_min; //pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg()); //pc.printf("rise state : %d\r\n",cyclic_servo.getState()); pc.printf("Sbdbtstate : %d\r\n",sbdbt.get_pairingState()); @@ -194,9 +193,10 @@ //cylinder void cylinder_origin_first() { - while(interrupt_cylinder_min == 1) { + while(interrupt_cylinder_min == 1){ rs422.put(5, -0.4, 0.0); } + rs422.put(5, 0.0, 0.0); enc_cylinder.origin(); } @@ -300,7 +300,7 @@ cylinder_origin(); //boost(); - led1 = sbdbt.get_pairingState(); + //led1 = sbdbt.get_pairingState(); sbdbt_indigator = sbdbt.get_pairingState(); if(sbdbt.up) { @@ -329,7 +329,7 @@ counter ++; break; case 3: - rs422.put(id[counter], shoulder_right_cal()*sholder_power,-1*shoulder_left_cal()*sholder_power); + rs422.put(id[counter], -1*shoulder_right_cal()*sholder_power,shoulder_left_cal()*sholder_power); counter ++; break; case 4: @@ -338,10 +338,31 @@ break; default: break; - }; + } }else{ - for(int count_i = 0;count_i<=nucleo_num;count_i++){ - rs422.put(id[count_i],0.0,0.0); + switch (counter) { + case 0: + rs422.put(id[counter],0.0,0.0); + counter++; + break; + case 1: + rs422.put(id[counter],0.0,0.0); + counter ++; + break; + case 2: + rs422.put(id[counter],0.0,0.0); + counter ++; + break; + case 3: + rs422.put(id[counter],0.0,0.0); + counter ++; + break; + case 4: + rs422.put(id[counter],0.0,0.0); + counter = 0; + break; + default: + break; } } } \ No newline at end of file