![](/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:
- 28:72b9af7e1700
- Parent:
- 27:2ffbc5587399
- Child:
- 29:f380201ca30f
--- a/main.cpp Thu Aug 24 16:40:04 2017 +0000 +++ b/main.cpp Thu Aug 24 17:50:01 2017 +0000 @@ -13,6 +13,7 @@ #include "cyclic.h" #include "cyclic_IO.h" #include "cylinder.h" +#include "Servo.h" //#include "QEI.h" #define pc_baud 460800 @@ -23,13 +24,14 @@ #define rs422_rx p27 #define rs422_baud 115200 #define output_period 0.015 -#define nucleo_num 5 +#define nucleo_num 6 #define pi 3.141592 #define n1_id 3 #define n2_id 4 #define n3_id 5 #define n4_id 6 #define n5_id 7 +#define n6_id 8 #define yaw_Kp 0.01 #define yaw_Ki 0.01 #define yaw_Kd 0.01 @@ -43,6 +45,8 @@ #define enc_Ki 0.01 #define enc_Kd 0.01 #define powerdown 0.6 +#define pin_servo_reload p21 // +#define pin_interrupt_reload p22 // DigitalOut led1(LED1); //DigitalOut led2(LED2); @@ -62,10 +66,11 @@ Accel v4; Cylinder cylinder(pin_cylinder_on,pin_cylinder_off); Cyclic sword; - //追加点 Encoder enc_cylinder(encoder_A,encoder_B); //QEI wheel(encoder_A, encoder_B, NC, 624); +Servo servo_reload(pin_servo_reload); +DigitalIn interrupt_reload(pin_interrupt_reload); void setup(); void output(); @@ -116,7 +121,7 @@ //boost(); static int counter; - int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id}; + int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id}; switch (counter) { case 0: @@ -156,10 +161,22 @@ led1 = 0; } +//追記(動作未確認) void cylinder_cal() { cylinder.cyclic(sbdbt.shikaku); enc_cylinder.deg_cylinder_cal(); + + if(cylinder.getInState()){ + if(interrupt_reload == 0){ + rs422.put(n6_id, 0.8, 0.0); + }else{ + servo_reload =+ 0.05; + if(servo_reload >= 1.0)servo_reload = 1.0; + } + }else{ + servo_reload = 0.0; + } } void boost(){