moves.cpp

00001 #include "moves.h"
00002 #include "pinnames.h"
00003 #include "sensors.h"
00004 #include "microinfinity.h"
00005 #include "debug.h"
00006 PwmOut motor_lo_f(pin_pwm[0][0]); //モータ正転
00007 PwmOut motor_lo_b(pin_pwm[0][1]); //モータ逆転
00008 PwmOut motor_li_f(pin_pwm[1][0]); //モータ正転
00009 PwmOut motor_li_b(pin_pwm[1][1]); //モータ逆転
00010 //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
00011 PIDcontroller pid_lo(0.01, 0.000, 0.000);
00012 PIDcontroller pid_li(0.01, 0.000, 0.000);    //Kp.Ki,Kd
00013 Motor motor_lo(&motor_lo_f, &motor_lo_b),
00014       motor_li(&motor_li_f, &motor_li_b);
00015 OneLeg leg_lo, leg_li;
00016 Robot robot;
00017 
00018 enum WalkMode {
00019     BACK,
00020     STOP,
00021     FORWARD,
00022 };
00023 int step_num_l, step_num_r;
00024 void WaitAnotherLeg();
00025 
00026 //収束していなければLED1をON. param:: d_step_l:左足を動かす歩数。負なら後ろに進む
00027 void Run(WalkMode mode, int d_step_l)
00028 {
00029     int target_step = step_num_l + d_step_l;
00030     can_send(mode, motor_lo.getDutyLimit());
00031     leg_lo.setTargetPose(180 + target_step*180);
00032     leg_li.setTargetPose(target_step*180);
00033     //収束前
00034     led1 = 1;
00035     robot.run();
00036     led1 = 0;
00037     step_num_l = target_step;
00038     switch(mode) {
00039         case FORWARD:
00040             step_num_r++;
00041             break;
00042         case STOP:
00043             break;
00044         case BACK:
00045             step_num_r--;
00046             break;
00047     }
00048 
00049     motor_lo_f.write(0);
00050     motor_lo_b.write(0);
00051     motor_li_f.write(0);
00052     motor_li_b.write(0);
00053     FileWrite();
00054     WaitAnotherLeg();
00055 }
00056 void WaitAnotherLeg()
00057 {
00058     while(1) {
00059         if(bus_in.read() == 1) break;
00060     }
00061 }
00062 void turn(float target_degree)//turn_degreeを超えるまで旋回し続ける関数
00063 {
00064     if(target_degree - degree0 > 0) {
00065         while(target_degree - degree0 > 0)
00066             turnLeft();
00067     } else {
00068         while(target_degree - degree0 < 0)
00069             turnRight();
00070     }
00071     DEBUG("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
00072 }
00073 void straightAndInfinity(float target_degree, float tolerance_degree)
00074 {
00075     if(target_degree - degree0 > tolerance_degree) curveLeft();
00076     else if(degree0 - target_degree > tolerance_degree) curveRight();
00077     else straight();
00078 }
00079 void climbAndInfinity(float target_degree, float tolerance_degree)
00080 {
00081     if(target_degree - degree0 > tolerance_degree) {//左に曲がる
00082         curveLeft();
00083     } else if(degree0 - target_degree > tolerance_degree) {//右に曲がる
00084         curveRight();
00085     } else {
00086         straight();
00087     }
00088 }
00089 
00090 void stop()
00091 {
00092     Run(STOP,0);
00093 }
00094 void straight()
00095 {
00096     Run(FORWARD,1);
00097 }
00098 void turnLeft()
00099 {
00100     Run(FORWARD,-1);
00101 }
00102 void curveLeft()
00103 {
00104     Run(FORWARD,0);
00105 }
00106 void turnRight()
00107 {
00108     Run(BACK,1);
00109 }
00110 void curveRight()
00111 {
00112     Run(STOP, 1);
00113 }
00114 void phasing(float target_degree)
00115 {
00116     if(step_num_l%2 != step_num_r%2) {
00117         if(target_degree - degree0 > 0) {
00118             curveLeft();
00119         } else {
00120             curveRight();
00121         }
00122     }
00123 }
00124 void SetupMoves()
00125 {
00126     motor_lo_f.period_us(100);
00127     motor_lo_b.period_us(100);
00128     motor_li_f.period_us(100);
00129     motor_li_b.period_us(100);
00130 
00131     pid_lo.setTolerance(10);
00132     pid_li.setTolerance(10);
00133     motor_lo.setEncoder(&ec_lo);
00134     motor_lo.setResolution(1000);
00135     motor_li.setEncoder(&ec_li);
00136     motor_li.setResolution(600);
00137     leg_lo.setMotor(&motor_lo);
00138     leg_lo.setPIDcontroller(&pid_lo);
00139     leg_li.setMotor(&motor_li);
00140     leg_li.setPIDcontroller(&pid_li);
00141     robot.setLeg(&leg_lo, &leg_li);
00142     robot.setTickerTime(0.01); //モータ出力間隔 0.01
00143 }
00144 
00145 void reset()
00146 {
00147     motor_lo.setDutyLimit(0.2);
00148     motor_li.setDutyLimit(0.2);
00149     while(switch_lo.read()) {
00150         motor_lo.output(0.2);
00151     }
00152     ec_lo.reset();
00153     motor_lo.output(0.0);
00154     while(switch_li.read()) {
00155         motor_li.output(0.2);
00156     }
00157     ec_li.reset();
00158     motor_li.output(0.0);
00159 }
00160 void HandMove()
00161 {
00162     can_send(STOP,0);
00163 }