ROBOSTEP_5期 / Mbed 2 deprecated George_Master_Param

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed robot

moves/moves.cpp@22:0ed9de464f40, 2019-05-03 (annotated)

Committer:
shimizuta
Date:
Fri May 03 09:34:05 2019 +0000
Revision:
22:0ed9de464f40
Child:
23:ef274a44a867
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 22:0ed9de464f40 1 #include "moves.h"
shimizuta 22:0ed9de464f40 2 #include "pinnames.h"
shimizuta 22:0ed9de464f40 3 #include "sensors.h"
shimizuta 22:0ed9de464f40 4 #include "microinfinity.h"
shimizuta 22:0ed9de464f40 5 PwmOut motor_lo_f(pin_pwm[0][0]); //モータ正転
shimizuta 22:0ed9de464f40 6 PwmOut motor_lo_b(pin_pwm[0][1]); //モータ逆転
shimizuta 22:0ed9de464f40 7 PwmOut motor_li_f(pin_pwm[1][0]); //モータ正転
shimizuta 22:0ed9de464f40 8 PwmOut motor_li_b(pin_pwm[1][1]); //モータ逆転
shimizuta 22:0ed9de464f40 9 //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
shimizuta 22:0ed9de464f40 10 PIDcontroller pid_lo(0.01, 0.000, 0.000);
shimizuta 22:0ed9de464f40 11 PIDcontroller pid_li(0.01, 0.000, 0.000); //Kp.Ki,Kd
shimizuta 22:0ed9de464f40 12 Motor motor_lo(&motor_lo_f, &motor_lo_b),
shimizuta 22:0ed9de464f40 13 motor_li(&motor_li_f, &motor_li_b);
shimizuta 22:0ed9de464f40 14 OneLeg leg_lo, leg_li;
shimizuta 22:0ed9de464f40 15 Robot robot;
shimizuta 22:0ed9de464f40 16
shimizuta 22:0ed9de464f40 17 enum WalkMode {
shimizuta 22:0ed9de464f40 18 BACK,
shimizuta 22:0ed9de464f40 19 STOP,
shimizuta 22:0ed9de464f40 20 FORWARD,
shimizuta 22:0ed9de464f40 21 };
shimizuta 22:0ed9de464f40 22 int step_num_l, step_num_r;
shimizuta 22:0ed9de464f40 23 void turn(float target_degree)//turn_degreeを超えるまで旋回し続ける関数
shimizuta 22:0ed9de464f40 24 {
shimizuta 22:0ed9de464f40 25 if(target_degree - degree0 > 0) {
shimizuta 22:0ed9de464f40 26 while(target_degree - degree0 > 0)
shimizuta 22:0ed9de464f40 27 turnLeft();
shimizuta 22:0ed9de464f40 28 } else {
shimizuta 22:0ed9de464f40 29 while(target_degree - degree0 < 0)
shimizuta 22:0ed9de464f40 30 turnRight();
shimizuta 22:0ed9de464f40 31 }
shimizuta 22:0ed9de464f40 32 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
shimizuta 22:0ed9de464f40 33 }
shimizuta 22:0ed9de464f40 34 void straightAndInfinity(float target_degree, float tolerance_degree)
shimizuta 22:0ed9de464f40 35 {
shimizuta 22:0ed9de464f40 36 if(target_degree - degree0 > tolerance_degree) curveLeft();
shimizuta 22:0ed9de464f40 37 else if(degree0 - target_degree > tolerance_degree) curveRight();
shimizuta 22:0ed9de464f40 38 else straight();
shimizuta 22:0ed9de464f40 39 }
shimizuta 22:0ed9de464f40 40 void straight()
shimizuta 22:0ed9de464f40 41 {
shimizuta 22:0ed9de464f40 42 can_send(FORWARD, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 43 leg_lo.setTargetPose(360+step_num_l*180);
shimizuta 22:0ed9de464f40 44 leg_li.setTargetPose(180+step_num_l*180);
shimizuta 22:0ed9de464f40 45 robot.run();
shimizuta 22:0ed9de464f40 46 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 47 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 48 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 49 motor_li_b.write(0);
shimizuta 22:0ed9de464f40 50 while(1) {
shimizuta 22:0ed9de464f40 51 if(bus_in.read() == 1) break;
shimizuta 22:0ed9de464f40 52 }
shimizuta 22:0ed9de464f40 53 step_num_l++;
shimizuta 22:0ed9de464f40 54 step_num_r++;
shimizuta 22:0ed9de464f40 55 }
shimizuta 22:0ed9de464f40 56 void turnLeft()
shimizuta 22:0ed9de464f40 57 {
shimizuta 22:0ed9de464f40 58 can_send(FORWARD, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 59 leg_lo.setTargetPose(360+(step_num_l-2)*180);
shimizuta 22:0ed9de464f40 60 leg_li.setTargetPose(180+(step_num_l-2)*180);
shimizuta 22:0ed9de464f40 61 robot.run();
shimizuta 22:0ed9de464f40 62 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 63 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 64 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 65 motor_li_b.write(0);
shimizuta 22:0ed9de464f40 66 while(1) {
shimizuta 22:0ed9de464f40 67 if(bus_in.read() == 1) break;
shimizuta 22:0ed9de464f40 68 }
shimizuta 22:0ed9de464f40 69 step_num_r++;
shimizuta 22:0ed9de464f40 70 step_num_l--;
shimizuta 22:0ed9de464f40 71 }
shimizuta 22:0ed9de464f40 72 void curveLeft()
shimizuta 22:0ed9de464f40 73 {
shimizuta 22:0ed9de464f40 74 can_send(FORWARD, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 75 leg_lo.setTargetPose(360+(step_num_l-1)*180);
shimizuta 22:0ed9de464f40 76 leg_li.setTargetPose(180+(step_num_l-1)*180);
shimizuta 22:0ed9de464f40 77 robot.run();
shimizuta 22:0ed9de464f40 78 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 79 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 80 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 81 motor_li_b.write(0);
shimizuta 22:0ed9de464f40 82 while(1) {
shimizuta 22:0ed9de464f40 83 if(bus_in.read() == 1) break;
shimizuta 22:0ed9de464f40 84 }
shimizuta 22:0ed9de464f40 85 step_num_r++;
shimizuta 22:0ed9de464f40 86 }
shimizuta 22:0ed9de464f40 87 void turnRight()
shimizuta 22:0ed9de464f40 88 {
shimizuta 22:0ed9de464f40 89 can_send(BACK, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 90 leg_lo.setTargetPose(360+step_num_l*180);
shimizuta 22:0ed9de464f40 91 leg_li.setTargetPose(180+step_num_l*180);
shimizuta 22:0ed9de464f40 92 robot.run();
shimizuta 22:0ed9de464f40 93 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 94 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 95 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 96 motor_li_b.write(0);
shimizuta 22:0ed9de464f40 97 while(1) {
shimizuta 22:0ed9de464f40 98 if(bus_in.read() == 1) break;
shimizuta 22:0ed9de464f40 99 }
shimizuta 22:0ed9de464f40 100 step_num_r--;
shimizuta 22:0ed9de464f40 101 step_num_l++;
shimizuta 22:0ed9de464f40 102 }
shimizuta 22:0ed9de464f40 103 void curveRight()
shimizuta 22:0ed9de464f40 104 {
shimizuta 22:0ed9de464f40 105 can_send(STOP, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 106 leg_lo.setTargetPose(360+step_num_l*180);
shimizuta 22:0ed9de464f40 107 leg_li.setTargetPose(180+step_num_l*180);
shimizuta 22:0ed9de464f40 108 robot.run();
shimizuta 22:0ed9de464f40 109 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 110 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 111 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 112 motor_li_b.write(0);
shimizuta 22:0ed9de464f40 113 while(1) {
shimizuta 22:0ed9de464f40 114 if(bus_in.read() == 1) break;
shimizuta 22:0ed9de464f40 115 }
shimizuta 22:0ed9de464f40 116 step_num_l++;
shimizuta 22:0ed9de464f40 117 }
shimizuta 22:0ed9de464f40 118 void SetupMoves()
shimizuta 22:0ed9de464f40 119 {
shimizuta 22:0ed9de464f40 120 motor_lo_f.period_us(100);
shimizuta 22:0ed9de464f40 121 motor_lo_b.period_us(100);
shimizuta 22:0ed9de464f40 122 motor_li_f.period_us(100);
shimizuta 22:0ed9de464f40 123 motor_li_b.period_us(100);
shimizuta 22:0ed9de464f40 124
shimizuta 22:0ed9de464f40 125 pid_lo.setTolerance(10);
shimizuta 22:0ed9de464f40 126 pid_li.setTolerance(10);
shimizuta 22:0ed9de464f40 127 motor_lo.setEncoder(&ec_lo);
shimizuta 22:0ed9de464f40 128 motor_lo.setResolution(1000);
shimizuta 22:0ed9de464f40 129 motor_li.setEncoder(&ec_li);
shimizuta 22:0ed9de464f40 130 motor_li.setResolution(600);
shimizuta 22:0ed9de464f40 131 leg_lo.setMotor(&motor_lo);
shimizuta 22:0ed9de464f40 132 leg_lo.setPIDcontroller(&pid_lo);
shimizuta 22:0ed9de464f40 133 leg_li.setMotor(&motor_li);
shimizuta 22:0ed9de464f40 134 leg_li.setPIDcontroller(&pid_li);
shimizuta 22:0ed9de464f40 135 robot.setLeg(&leg_lo, &leg_li);
shimizuta 22:0ed9de464f40 136 robot.setTickerTime(0.01); //モータ出力間隔 0.01
shimizuta 22:0ed9de464f40 137 }
shimizuta 22:0ed9de464f40 138
shimizuta 22:0ed9de464f40 139 void reset()
shimizuta 22:0ed9de464f40 140 {
shimizuta 22:0ed9de464f40 141 while(switch_lo.read()) {
shimizuta 22:0ed9de464f40 142 motor_lo.output(0.3);
shimizuta 22:0ed9de464f40 143 }
shimizuta 22:0ed9de464f40 144 ec_lo.reset();
shimizuta 22:0ed9de464f40 145 motor_lo.output(0.0);
shimizuta 22:0ed9de464f40 146 while(switch_li.read()) {
shimizuta 22:0ed9de464f40 147 motor_li.output(0.3);
shimizuta 22:0ed9de464f40 148 }
shimizuta 22:0ed9de464f40 149 ec_li.reset();
shimizuta 22:0ed9de464f40 150 motor_li.output(0.0);
shimizuta 22:0ed9de464f40 151 }
shimizuta 22:0ed9de464f40 152
shimizuta 22:0ed9de464f40 153
shimizuta 22:0ed9de464f40 154
shimizuta 22:0ed9de464f40 155
shimizuta 22:0ed9de464f40 156