sampleProgram
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_fuzi by
Diff: main.cpp
- Revision:
- 66:1664ee92539d
- Parent:
- 65:5e4c2e5494ae
- Child:
- 67:b094d88583be
--- a/main.cpp Sun Sep 24 01:28:34 2017 +0000 +++ b/main.cpp Sun Sep 24 05:25:03 2017 +0000 @@ -1,8 +1,8 @@ //index_include #include "mbed.h" #include "math.h" -#include "bit_test.h" -#include "RS422_put.h" +//#include "bit_test.h" +#include "rs422_put.h" #include "sbdbt.h" #include "mecanum.h" #include "bno055_lib.h" @@ -10,11 +10,11 @@ #include "pid.h" #include "limit.h" #include "accelerator.h" -#include "encorder.h" -#include "cyclic.h" -#include "cyclic_IO.h" +#include "pid_encoder.h" +#include "cyclic_var.h" +#include "cyclic_io.h" #include "cylinder.h" -#include "varEvent.h" +#include "event_var.h" //index_define #define pc_baud 460800 @@ -67,30 +67,30 @@ DigitalOut led4(LED4); DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min); Serial pc(USBTX,USBRX); -RS422 rs422(rs422_tx, rs422_rx); +Rs422 rs422(rs422_tx, rs422_rx); Sbdbt sbdbt(sbdbt_tx, sbdbt_rx, pin_sbdbt_pairing); Ticker output_timer; Mecanum mecanum; Bno055 bno055; -Position_pid yaw_pid; -Accel v1; -Accel v2; -Accel v3; -Accel v4; +PositionPid yaw_pid; +Accelerator v1; +Accelerator v2; +Accelerator v3; +Accelerator v4; DigitalOut cylinder_on(pin_cylinder_on); DigitalOut cylinder_off(pin_cylinder_off); -Cyclic sword; -Cyclic cyclic_cylinder_position; +CyclicVar sword; +CyclicVar cyclic_cylinder_position; DigitalIn interrupt_sholderright_min(pin_interrupt_sholderright_min); DigitalIn interrupt_sholderright_max(pin_interrupt_sholderright_max); DigitalIn interrupt_sholderleft_min(pin_interrupt_sholderleft_min); DigitalIn interrupt_sholderleft_max(pin_interrupt_sholderleft_max); Encoder enc_cylinder(encoder_A,encoder_B); -Cyclic cyclic_servo; -Cyclic_IO cylinder_reload(pin_cylinder_reload); +CyclicVar cyclic_servo; +CyclicIo cylinder_reload(pin_cylinder_reload); //PwmOut servo(pin_servo); DigitalOut sbdbt_indigator(pin_sbdbt_indicator); -varEvent cylinder_event; +eventVar cylinder_event; //index_setupFunction void setup(); @@ -141,14 +141,14 @@ firstmotion(); cylinder_pos_num = 2; //セットアップタイムでの初期装填のため output_timer.attach(&output, output_period); - yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd); + yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd,output_period); mecanum.setupdeg(bno055.getYawRad()); //基盤が前後逆の場合+180 v1.setup(acceleration,output_period); v2.setup(acceleration,output_period); v3.setup(acceleration,output_period); v4.setup(acceleration,output_period); - enc_cylinder.setup(100); - enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd); + enc_cylinder.setPpr(100); + enc_cylinder.setup(enc_Kp,enc_Ki,enc_Kd,output_period); //servo.period(0.020); } @@ -287,7 +287,7 @@ static float out_right_x; yaw = bno055.getYawRad(); target_yaw = yaw; - yaw_pid.cal(target_yaw, yaw, output_period); + yaw_pid.calculate(target_yaw, yaw); //後進時に方向が逆転するため if(sbdbt.right_y < 0.0){