yuto kawamura
/
George_Slave_BOTHMOVE
right and left move at the same time
main.cpp
- Committer:
- yuto17320508
- Date:
- 2019-05-15
- Revision:
- 13:678870d8f851
- Parent:
- 12:e6fd3a3201f0
File content as of revision 13:678870d8f851:
#include "mbed.h" #include "pin.h" #include "debug.h" #include "robot.h" enum WALKMODE { BACK, STOP, FORWARD, }; enum EVENT { G_OPEN, G_CLOSE, GOAL, }; float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。 int hand_mode=G_CLOSE; void setup(); void forward(); void stop(); void back(); void can_receive(int &mode, float &duty); void reset(); //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う //しかし変更を多々行うためポインタ渡しにしてある //文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う PIDcontroller pid_ro(0.01, 0.000, 0.000); PIDcontroller pid_ri(0.01, 0.000, 0.000); //Kp.Ki,Kd Motor motor_ro(&motor_ro_f, &motor_ro_b), motor_ri(&motor_ri_f, &motor_ri_b); //forward,backのピンを代入 OneLeg leg_ro, leg_ri; Robot robot; DigitalOut led[] = {LED1,LED2,LED3,LED4}; int step_num_r = 0; int main() { printf("standby ok\n\r"); setup(); /*while(1){ servo.set_degree(0,(7200 - 3500) * 270.0/(11500 - 3500)); wait(0.01); printf("servo"); }*/ pid_ro.setTolerance(10); pid_ri.setTolerance(10); motor_ro.setEncoder(&ec_ro); motor_ro.setResolution(1000); motor_ri.setEncoder(&ec_ri); motor_ri.setResolution(1000); leg_ro.setMotor(&motor_ro); leg_ro.setPIDcontroller(&pid_ro); leg_ri.setMotor(&motor_ri); leg_ri.setPIDcontroller(&pid_ri); robot.setLeg(&leg_ro, &leg_ri); robot.setTickerTime(0.01); //モータ出力間隔 0.01 motor_ro.setDutyLimit(0.5); motor_ri.setDutyLimit(0.5); reset(); for(int i=0; i<5; ++i) servo.set_degree(0,0); bus_out = 1; printf("start\n\r"); int mode = -1; air_hand = 0; while(1) { float duty = 0.0; can_receive(mode,duty); bus_out = 0;//未収束 printf("mode:%d duty:%.3f\n\r", mode, duty); motor_ro.setDutyLimit(duty); motor_ri.setDutyLimit(duty); //printf("target is %d\n\r",target_ro); if(mode == FORWARD) forward(); else if(mode == STOP) stop(); else if(mode == BACK) back(); if(hand_mode == G_CLOSE) { air_hand = 0; } else if(hand_mode == G_OPEN) { air_hand = 1; } else if(hand_mode == GOAL) { for(int i=0; i<10; ++i) servo.set_degree(0,(7200 - 3500) * 270.0/(11500 - 3500)); } bus_out = 1;//収束 } } void Run(int d_step_r) { int target_step = step_num_r + d_step_r; leg_ro.setTargetPose(180+target_step*180); leg_ri.setTargetPose(target_step*180); led[0] = 1; robot.run(); led[0] = 0; step_num_r = target_step; motor_ro_f.write(0); motor_ro_b.write(0); motor_ri_f.write(0); motor_ri_b.write(0); } void forward() { Run(1); } void stop() { Run(0); } void back() { Run(-1); } ////////////////////////////////////////////// void setup() { can.frequency(1000000); motor_ro_f.period_us(100); motor_ro_b.period_us(100); motor_ri_f.period_us(100); motor_ri_b.period_us(100); air_hand = 0;//ゲレゲを離す手の形にしておく switch_ro.mode(PullUp); switch_ri.mode(PullUp); servo.init(); wait(0.01); for(int i=0; i<5; ++i) servo.set_degree(0,0); //(7200 - 3500) * 270.0/(11500 - 3500) } void reset() { while(switch_ro.read()) { motor_ro.output(0.3); } ec_ro.reset(); motor_ro.output(0.0); printf("ro OK\n\r"); while(switch_ri.read()) { motor_ri.output(0.3); } ec_ri.reset(); motor_ri.output(0.0); printf("ri OK\n\r"); } void can_receive(int &mode, float &duty) { CANMessage msg; while(1) { //printf("roop\r\n"); if(can.read(msg)) { if(msg.id==0) { mode= msg.data[0]*0.1; duty = (msg.data[0]%10)*0.1; hand_mode= msg.data[1]; break; } led2=1; } else led2=0; } }