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@25:c740e6fd5dab, 2019-05-06 (annotated)

Committer:
yuto17320508
Date:
Mon May 06 04:03:00 2019 +0000
Revision:
25:c740e6fd5dab
Parent:
23:ef274a44a867
Child:
26:5fb1aa9cb7f0
l

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 23:ef274a44a867 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 }
yuto17320508 25:c740e6fd5dab 40 void climbAndInfinity(float target_degree, float tolerance_degree)
yuto17320508 25:c740e6fd5dab 41 {
yuto17320508 25:c740e6fd5dab 42 if(target_degree - degree0 > tolerance_degree) {//左に曲がる
yuto17320508 25:c740e6fd5dab 43
yuto17320508 25:c740e6fd5dab 44 curveLeft();
yuto17320508 25:c740e6fd5dab 45 } else if(degree0 - target_degree > tolerance_degree) {//右に曲がる
yuto17320508 25:c740e6fd5dab 46
yuto17320508 25:c740e6fd5dab 47 curveRight();
yuto17320508 25:c740e6fd5dab 48 } else {
yuto17320508 25:c740e6fd5dab 49
yuto17320508 25:c740e6fd5dab 50 straight();
yuto17320508 25:c740e6fd5dab 51 }
yuto17320508 25:c740e6fd5dab 52 }
yuto17320508 25:c740e6fd5dab 53 void onestraight()
yuto17320508 25:c740e6fd5dab 54 {
yuto17320508 25:c740e6fd5dab 55 leg_lo.setTargetPose(360+step_num_l*180);
yuto17320508 25:c740e6fd5dab 56 leg_li.setTargetPose(180+step_num_l*180);
yuto17320508 25:c740e6fd5dab 57 robot.run();
yuto17320508 25:c740e6fd5dab 58 motor_lo_f.write(0);
yuto17320508 25:c740e6fd5dab 59 motor_lo_b.write(0);
yuto17320508 25:c740e6fd5dab 60 motor_li_f.write(0);
yuto17320508 25:c740e6fd5dab 61 motor_li_b.write(0);
yuto17320508 25:c740e6fd5dab 62 step_num_l++;
yuto17320508 25:c740e6fd5dab 63 step_num_r++;
yuto17320508 25:c740e6fd5dab 64 }
shimizuta 22:0ed9de464f40 65 void straight()
shimizuta 22:0ed9de464f40 66 {
shimizuta 22:0ed9de464f40 67 can_send(FORWARD, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 68 leg_lo.setTargetPose(360+step_num_l*180);
shimizuta 22:0ed9de464f40 69 leg_li.setTargetPose(180+step_num_l*180);
shimizuta 22:0ed9de464f40 70 robot.run();
shimizuta 22:0ed9de464f40 71 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 72 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 73 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 74 motor_li_b.write(0);
shimizuta 22:0ed9de464f40 75 while(1) {
shimizuta 22:0ed9de464f40 76 if(bus_in.read() == 1) break;
shimizuta 22:0ed9de464f40 77 }
shimizuta 22:0ed9de464f40 78 step_num_l++;
shimizuta 22:0ed9de464f40 79 step_num_r++;
shimizuta 22:0ed9de464f40 80 }
shimizuta 22:0ed9de464f40 81 void turnLeft()
shimizuta 22:0ed9de464f40 82 {
shimizuta 22:0ed9de464f40 83 can_send(FORWARD, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 84 leg_lo.setTargetPose(360+(step_num_l-2)*180);
shimizuta 22:0ed9de464f40 85 leg_li.setTargetPose(180+(step_num_l-2)*180);
shimizuta 22:0ed9de464f40 86 robot.run();
shimizuta 22:0ed9de464f40 87 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 88 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 89 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 90 motor_li_b.write(0);
shimizuta 22:0ed9de464f40 91 while(1) {
shimizuta 22:0ed9de464f40 92 if(bus_in.read() == 1) break;
shimizuta 22:0ed9de464f40 93 }
shimizuta 22:0ed9de464f40 94 step_num_r++;
shimizuta 22:0ed9de464f40 95 step_num_l--;
shimizuta 22:0ed9de464f40 96 }
shimizuta 22:0ed9de464f40 97 void curveLeft()
shimizuta 22:0ed9de464f40 98 {
shimizuta 22:0ed9de464f40 99 can_send(FORWARD, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 100 leg_lo.setTargetPose(360+(step_num_l-1)*180);
shimizuta 22:0ed9de464f40 101 leg_li.setTargetPose(180+(step_num_l-1)*180);
shimizuta 22:0ed9de464f40 102 robot.run();
shimizuta 22:0ed9de464f40 103 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 104 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 105 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 106 motor_li_b.write(0);
shimizuta 22:0ed9de464f40 107 while(1) {
shimizuta 22:0ed9de464f40 108 if(bus_in.read() == 1) break;
shimizuta 22:0ed9de464f40 109 }
yuto17320508 25:c740e6fd5dab 110 }
yuto17320508 25:c740e6fd5dab 111 void backLeft()
yuto17320508 25:c740e6fd5dab 112 {
yuto17320508 25:c740e6fd5dab 113 can_send(STOP, motor_lo.getDutyLimit());
yuto17320508 25:c740e6fd5dab 114 leg_lo.setTargetPose(360+(step_num_l-2)*180);
yuto17320508 25:c740e6fd5dab 115 leg_li.setTargetPose(180+(step_num_l-2)*180);
yuto17320508 25:c740e6fd5dab 116 robot.run();
yuto17320508 25:c740e6fd5dab 117 motor_lo_f.write(0);
yuto17320508 25:c740e6fd5dab 118 motor_lo_b.write(0);
yuto17320508 25:c740e6fd5dab 119 motor_li_f.write(0);
yuto17320508 25:c740e6fd5dab 120 motor_li_b.write(0);
yuto17320508 25:c740e6fd5dab 121 while(1) {
yuto17320508 25:c740e6fd5dab 122 if(bus_in.read() == 1) break;
yuto17320508 25:c740e6fd5dab 123 }
yuto17320508 25:c740e6fd5dab 124 step_num_l--;
shimizuta 22:0ed9de464f40 125 }
shimizuta 22:0ed9de464f40 126 void turnRight()
shimizuta 22:0ed9de464f40 127 {
shimizuta 22:0ed9de464f40 128 can_send(BACK, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 129 leg_lo.setTargetPose(360+step_num_l*180);
shimizuta 22:0ed9de464f40 130 leg_li.setTargetPose(180+step_num_l*180);
shimizuta 22:0ed9de464f40 131 robot.run();
shimizuta 22:0ed9de464f40 132 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 133 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 134 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 135 motor_li_b.write(0);
shimizuta 22:0ed9de464f40 136 while(1) {
shimizuta 22:0ed9de464f40 137 if(bus_in.read() == 1) break;
shimizuta 22:0ed9de464f40 138 }
shimizuta 22:0ed9de464f40 139 step_num_r--;
shimizuta 22:0ed9de464f40 140 step_num_l++;
shimizuta 22:0ed9de464f40 141 }
shimizuta 22:0ed9de464f40 142 void curveRight()
shimizuta 22:0ed9de464f40 143 {
shimizuta 22:0ed9de464f40 144 can_send(STOP, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 145 leg_lo.setTargetPose(360+step_num_l*180);
shimizuta 22:0ed9de464f40 146 leg_li.setTargetPose(180+step_num_l*180);
shimizuta 22:0ed9de464f40 147 robot.run();
shimizuta 22:0ed9de464f40 148 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 149 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 150 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 151 motor_li_b.write(0);
shimizuta 22:0ed9de464f40 152 while(1) {
shimizuta 22:0ed9de464f40 153 if(bus_in.read() == 1) break;
shimizuta 22:0ed9de464f40 154 }
shimizuta 22:0ed9de464f40 155 step_num_l++;
shimizuta 22:0ed9de464f40 156 }
yuto17320508 25:c740e6fd5dab 157 void backRight()
yuto17320508 25:c740e6fd5dab 158 {
yuto17320508 25:c740e6fd5dab 159 can_send(BACK, motor_lo.getDutyLimit());
yuto17320508 25:c740e6fd5dab 160 leg_lo.setTargetPose(360+(step_num_l-1)*180);
yuto17320508 25:c740e6fd5dab 161 leg_li.setTargetPose(180+(step_num_l-1)*180);
yuto17320508 25:c740e6fd5dab 162 robot.run();
yuto17320508 25:c740e6fd5dab 163 motor_lo_f.write(0);
yuto17320508 25:c740e6fd5dab 164 motor_lo_b.write(0);
yuto17320508 25:c740e6fd5dab 165 motor_li_f.write(0);
yuto17320508 25:c740e6fd5dab 166 motor_li_b.write(0);
yuto17320508 25:c740e6fd5dab 167 while(1) {
yuto17320508 25:c740e6fd5dab 168 if(bus_in.read() == 1) break;
yuto17320508 25:c740e6fd5dab 169 }
yuto17320508 25:c740e6fd5dab 170 }
shimizuta 22:0ed9de464f40 171 void SetupMoves()
shimizuta 22:0ed9de464f40 172 {
shimizuta 22:0ed9de464f40 173 motor_lo_f.period_us(100);
shimizuta 22:0ed9de464f40 174 motor_lo_b.period_us(100);
shimizuta 22:0ed9de464f40 175 motor_li_f.period_us(100);
shimizuta 22:0ed9de464f40 176 motor_li_b.period_us(100);
shimizuta 22:0ed9de464f40 177
shimizuta 22:0ed9de464f40 178 pid_lo.setTolerance(10);
shimizuta 22:0ed9de464f40 179 pid_li.setTolerance(10);
shimizuta 22:0ed9de464f40 180 motor_lo.setEncoder(&ec_lo);
shimizuta 22:0ed9de464f40 181 motor_lo.setResolution(1000);
shimizuta 22:0ed9de464f40 182 motor_li.setEncoder(&ec_li);
shimizuta 22:0ed9de464f40 183 motor_li.setResolution(600);
shimizuta 22:0ed9de464f40 184 leg_lo.setMotor(&motor_lo);
shimizuta 22:0ed9de464f40 185 leg_lo.setPIDcontroller(&pid_lo);
shimizuta 22:0ed9de464f40 186 leg_li.setMotor(&motor_li);
shimizuta 22:0ed9de464f40 187 leg_li.setPIDcontroller(&pid_li);
shimizuta 22:0ed9de464f40 188 robot.setLeg(&leg_lo, &leg_li);
shimizuta 22:0ed9de464f40 189 robot.setTickerTime(0.01); //モータ出力間隔 0.01
shimizuta 22:0ed9de464f40 190 }
shimizuta 22:0ed9de464f40 191
shimizuta 22:0ed9de464f40 192 void reset()
shimizuta 22:0ed9de464f40 193 {
yuto17320508 25:c740e6fd5dab 194 motor_lo.setDutyLimit(0.2);
yuto17320508 25:c740e6fd5dab 195 motor_li.setDutyLimit(0.2);
shimizuta 22:0ed9de464f40 196 while(switch_lo.read()) {
yuto17320508 25:c740e6fd5dab 197 motor_lo.output(0.2);
shimizuta 22:0ed9de464f40 198 }
shimizuta 22:0ed9de464f40 199 ec_lo.reset();
shimizuta 22:0ed9de464f40 200 motor_lo.output(0.0);
shimizuta 22:0ed9de464f40 201 while(switch_li.read()) {
yuto17320508 25:c740e6fd5dab 202 motor_li.output(0.2);
shimizuta 22:0ed9de464f40 203 }
shimizuta 22:0ed9de464f40 204 ec_li.reset();
shimizuta 22:0ed9de464f40 205 motor_li.output(0.0);
shimizuta 22:0ed9de464f40 206 }
shimizuta 22:0ed9de464f40 207
shimizuta 22:0ed9de464f40 208
shimizuta 22:0ed9de464f40 209
shimizuta 22:0ed9de464f40 210
shimizuta 22:0ed9de464f40 211