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@26:5fb1aa9cb7f0, 2019-05-06 (annotated)

Committer:
shimizuta
Date:
Mon May 06 06:28:41 2019 +0000
Revision:
26:5fb1aa9cb7f0
Parent:
25:c740e6fd5dab
Child:
27:d392a95f4799
can get log

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 26:5fb1aa9cb7f0 5 #include "debug.h"
shimizuta 22:0ed9de464f40 6 PwmOut motor_lo_f(pin_pwm[0][0]); //モータ正転
shimizuta 22:0ed9de464f40 7 PwmOut motor_lo_b(pin_pwm[0][1]); //モータ逆転
shimizuta 22:0ed9de464f40 8 PwmOut motor_li_f(pin_pwm[1][0]); //モータ正転
shimizuta 22:0ed9de464f40 9 PwmOut motor_li_b(pin_pwm[1][1]); //モータ逆転
shimizuta 22:0ed9de464f40 10 //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
shimizuta 22:0ed9de464f40 11 PIDcontroller pid_lo(0.01, 0.000, 0.000);
shimizuta 22:0ed9de464f40 12 PIDcontroller pid_li(0.01, 0.000, 0.000); //Kp.Ki,Kd
shimizuta 22:0ed9de464f40 13 Motor motor_lo(&motor_lo_f, &motor_lo_b),
shimizuta 23:ef274a44a867 14 motor_li(&motor_li_f, &motor_li_b);
shimizuta 22:0ed9de464f40 15 OneLeg leg_lo, leg_li;
shimizuta 22:0ed9de464f40 16 Robot robot;
shimizuta 22:0ed9de464f40 17
shimizuta 22:0ed9de464f40 18 enum WalkMode {
shimizuta 22:0ed9de464f40 19 BACK,
shimizuta 22:0ed9de464f40 20 STOP,
shimizuta 22:0ed9de464f40 21 FORWARD,
shimizuta 22:0ed9de464f40 22 };
shimizuta 22:0ed9de464f40 23 int step_num_l, step_num_r;
shimizuta 26:5fb1aa9cb7f0 24 void WaitAnotherLeg()
shimizuta 26:5fb1aa9cb7f0 25 {
shimizuta 26:5fb1aa9cb7f0 26 FileWrite();
shimizuta 26:5fb1aa9cb7f0 27 while(1) {
shimizuta 26:5fb1aa9cb7f0 28 if(bus_in.read() == 1) break;
shimizuta 26:5fb1aa9cb7f0 29 }
shimizuta 26:5fb1aa9cb7f0 30 }
shimizuta 22:0ed9de464f40 31 void turn(float target_degree)//turn_degreeを超えるまで旋回し続ける関数
shimizuta 22:0ed9de464f40 32 {
shimizuta 22:0ed9de464f40 33 if(target_degree - degree0 > 0) {
shimizuta 22:0ed9de464f40 34 while(target_degree - degree0 > 0)
shimizuta 22:0ed9de464f40 35 turnLeft();
shimizuta 22:0ed9de464f40 36 } else {
shimizuta 22:0ed9de464f40 37 while(target_degree - degree0 < 0)
shimizuta 22:0ed9de464f40 38 turnRight();
shimizuta 22:0ed9de464f40 39 }
shimizuta 26:5fb1aa9cb7f0 40 DEBUG("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
shimizuta 22:0ed9de464f40 41 }
shimizuta 22:0ed9de464f40 42 void straightAndInfinity(float target_degree, float tolerance_degree)
shimizuta 22:0ed9de464f40 43 {
shimizuta 22:0ed9de464f40 44 if(target_degree - degree0 > tolerance_degree) curveLeft();
shimizuta 22:0ed9de464f40 45 else if(degree0 - target_degree > tolerance_degree) curveRight();
shimizuta 22:0ed9de464f40 46 else straight();
shimizuta 22:0ed9de464f40 47 }
yuto17320508 25:c740e6fd5dab 48 void climbAndInfinity(float target_degree, float tolerance_degree)
yuto17320508 25:c740e6fd5dab 49 {
yuto17320508 25:c740e6fd5dab 50 if(target_degree - degree0 > tolerance_degree) {//左に曲がる
shimizuta 26:5fb1aa9cb7f0 51
yuto17320508 25:c740e6fd5dab 52 curveLeft();
yuto17320508 25:c740e6fd5dab 53 } else if(degree0 - target_degree > tolerance_degree) {//右に曲がる
shimizuta 26:5fb1aa9cb7f0 54
yuto17320508 25:c740e6fd5dab 55 curveRight();
yuto17320508 25:c740e6fd5dab 56 } else {
shimizuta 26:5fb1aa9cb7f0 57
yuto17320508 25:c740e6fd5dab 58 straight();
yuto17320508 25:c740e6fd5dab 59 }
yuto17320508 25:c740e6fd5dab 60 }
yuto17320508 25:c740e6fd5dab 61 void onestraight()
yuto17320508 25:c740e6fd5dab 62 {
yuto17320508 25:c740e6fd5dab 63 leg_lo.setTargetPose(360+step_num_l*180);
yuto17320508 25:c740e6fd5dab 64 leg_li.setTargetPose(180+step_num_l*180);
yuto17320508 25:c740e6fd5dab 65 robot.run();
yuto17320508 25:c740e6fd5dab 66 motor_lo_f.write(0);
yuto17320508 25:c740e6fd5dab 67 motor_lo_b.write(0);
yuto17320508 25:c740e6fd5dab 68 motor_li_f.write(0);
yuto17320508 25:c740e6fd5dab 69 motor_li_b.write(0);
yuto17320508 25:c740e6fd5dab 70 step_num_l++;
yuto17320508 25:c740e6fd5dab 71 step_num_r++;
yuto17320508 25:c740e6fd5dab 72 }
shimizuta 22:0ed9de464f40 73 void straight()
shimizuta 22:0ed9de464f40 74 {
shimizuta 22:0ed9de464f40 75 can_send(FORWARD, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 76 leg_lo.setTargetPose(360+step_num_l*180);
shimizuta 22:0ed9de464f40 77 leg_li.setTargetPose(180+step_num_l*180);
shimizuta 22:0ed9de464f40 78 robot.run();
shimizuta 22:0ed9de464f40 79 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 80 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 81 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 82 motor_li_b.write(0);
shimizuta 26:5fb1aa9cb7f0 83 WaitAnotherLeg();
shimizuta 22:0ed9de464f40 84 step_num_l++;
shimizuta 22:0ed9de464f40 85 step_num_r++;
shimizuta 22:0ed9de464f40 86 }
shimizuta 22:0ed9de464f40 87 void turnLeft()
shimizuta 22:0ed9de464f40 88 {
shimizuta 22:0ed9de464f40 89 can_send(FORWARD, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 90 leg_lo.setTargetPose(360+(step_num_l-2)*180);
shimizuta 22:0ed9de464f40 91 leg_li.setTargetPose(180+(step_num_l-2)*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 26:5fb1aa9cb7f0 97 WaitAnotherLeg();
shimizuta 22:0ed9de464f40 98 step_num_r++;
shimizuta 22:0ed9de464f40 99 step_num_l--;
shimizuta 22:0ed9de464f40 100 }
shimizuta 22:0ed9de464f40 101 void curveLeft()
shimizuta 22:0ed9de464f40 102 {
shimizuta 22:0ed9de464f40 103 can_send(FORWARD, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 104 leg_lo.setTargetPose(360+(step_num_l-1)*180);
shimizuta 22:0ed9de464f40 105 leg_li.setTargetPose(180+(step_num_l-1)*180);
shimizuta 22:0ed9de464f40 106 robot.run();
shimizuta 22:0ed9de464f40 107 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 108 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 109 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 110 motor_li_b.write(0);
shimizuta 26:5fb1aa9cb7f0 111 WaitAnotherLeg();
yuto17320508 25:c740e6fd5dab 112 }
yuto17320508 25:c740e6fd5dab 113 void backLeft()
yuto17320508 25:c740e6fd5dab 114 {
yuto17320508 25:c740e6fd5dab 115 can_send(STOP, motor_lo.getDutyLimit());
yuto17320508 25:c740e6fd5dab 116 leg_lo.setTargetPose(360+(step_num_l-2)*180);
yuto17320508 25:c740e6fd5dab 117 leg_li.setTargetPose(180+(step_num_l-2)*180);
yuto17320508 25:c740e6fd5dab 118 robot.run();
yuto17320508 25:c740e6fd5dab 119 motor_lo_f.write(0);
yuto17320508 25:c740e6fd5dab 120 motor_lo_b.write(0);
yuto17320508 25:c740e6fd5dab 121 motor_li_f.write(0);
yuto17320508 25:c740e6fd5dab 122 motor_li_b.write(0);
shimizuta 26:5fb1aa9cb7f0 123 WaitAnotherLeg();
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 26:5fb1aa9cb7f0 136 WaitAnotherLeg();
shimizuta 22:0ed9de464f40 137 step_num_r--;
shimizuta 22:0ed9de464f40 138 step_num_l++;
shimizuta 22:0ed9de464f40 139 }
shimizuta 22:0ed9de464f40 140 void curveRight()
shimizuta 22:0ed9de464f40 141 {
shimizuta 22:0ed9de464f40 142 can_send(STOP, motor_lo.getDutyLimit());
shimizuta 22:0ed9de464f40 143 leg_lo.setTargetPose(360+step_num_l*180);
shimizuta 22:0ed9de464f40 144 leg_li.setTargetPose(180+step_num_l*180);
shimizuta 22:0ed9de464f40 145 robot.run();
shimizuta 22:0ed9de464f40 146 motor_lo_f.write(0);
shimizuta 22:0ed9de464f40 147 motor_lo_b.write(0);
shimizuta 22:0ed9de464f40 148 motor_li_f.write(0);
shimizuta 22:0ed9de464f40 149 motor_li_b.write(0);
shimizuta 26:5fb1aa9cb7f0 150 WaitAnotherLeg();
shimizuta 22:0ed9de464f40 151 step_num_l++;
shimizuta 22:0ed9de464f40 152 }
yuto17320508 25:c740e6fd5dab 153 void backRight()
yuto17320508 25:c740e6fd5dab 154 {
yuto17320508 25:c740e6fd5dab 155 can_send(BACK, motor_lo.getDutyLimit());
yuto17320508 25:c740e6fd5dab 156 leg_lo.setTargetPose(360+(step_num_l-1)*180);
yuto17320508 25:c740e6fd5dab 157 leg_li.setTargetPose(180+(step_num_l-1)*180);
yuto17320508 25:c740e6fd5dab 158 robot.run();
yuto17320508 25:c740e6fd5dab 159 motor_lo_f.write(0);
yuto17320508 25:c740e6fd5dab 160 motor_lo_b.write(0);
yuto17320508 25:c740e6fd5dab 161 motor_li_f.write(0);
yuto17320508 25:c740e6fd5dab 162 motor_li_b.write(0);
shimizuta 26:5fb1aa9cb7f0 163 WaitAnotherLeg();
yuto17320508 25:c740e6fd5dab 164 }
shimizuta 22:0ed9de464f40 165 void SetupMoves()
shimizuta 22:0ed9de464f40 166 {
shimizuta 22:0ed9de464f40 167 motor_lo_f.period_us(100);
shimizuta 22:0ed9de464f40 168 motor_lo_b.period_us(100);
shimizuta 22:0ed9de464f40 169 motor_li_f.period_us(100);
shimizuta 22:0ed9de464f40 170 motor_li_b.period_us(100);
shimizuta 22:0ed9de464f40 171
shimizuta 22:0ed9de464f40 172 pid_lo.setTolerance(10);
shimizuta 22:0ed9de464f40 173 pid_li.setTolerance(10);
shimizuta 22:0ed9de464f40 174 motor_lo.setEncoder(&ec_lo);
shimizuta 22:0ed9de464f40 175 motor_lo.setResolution(1000);
shimizuta 22:0ed9de464f40 176 motor_li.setEncoder(&ec_li);
shimizuta 22:0ed9de464f40 177 motor_li.setResolution(600);
shimizuta 22:0ed9de464f40 178 leg_lo.setMotor(&motor_lo);
shimizuta 22:0ed9de464f40 179 leg_lo.setPIDcontroller(&pid_lo);
shimizuta 22:0ed9de464f40 180 leg_li.setMotor(&motor_li);
shimizuta 22:0ed9de464f40 181 leg_li.setPIDcontroller(&pid_li);
shimizuta 22:0ed9de464f40 182 robot.setLeg(&leg_lo, &leg_li);
shimizuta 22:0ed9de464f40 183 robot.setTickerTime(0.01); //モータ出力間隔 0.01
shimizuta 22:0ed9de464f40 184 }
shimizuta 22:0ed9de464f40 185
shimizuta 22:0ed9de464f40 186 void reset()
shimizuta 22:0ed9de464f40 187 {
yuto17320508 25:c740e6fd5dab 188 motor_lo.setDutyLimit(0.2);
yuto17320508 25:c740e6fd5dab 189 motor_li.setDutyLimit(0.2);
shimizuta 22:0ed9de464f40 190 while(switch_lo.read()) {
yuto17320508 25:c740e6fd5dab 191 motor_lo.output(0.2);
shimizuta 22:0ed9de464f40 192 }
shimizuta 22:0ed9de464f40 193 ec_lo.reset();
shimizuta 22:0ed9de464f40 194 motor_lo.output(0.0);
shimizuta 22:0ed9de464f40 195 while(switch_li.read()) {
yuto17320508 25:c740e6fd5dab 196 motor_li.output(0.2);
shimizuta 22:0ed9de464f40 197 }
shimizuta 22:0ed9de464f40 198 ec_li.reset();
shimizuta 22:0ed9de464f40 199 motor_li.output(0.0);
shimizuta 22:0ed9de464f40 200 }
shimizuta 22:0ed9de464f40 201
shimizuta 22:0ed9de464f40 202
shimizuta 22:0ed9de464f40 203
shimizuta 22:0ed9de464f40 204
shimizuta 22:0ed9de464f40 205