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.
main.cpp@18:d0a6771fa38d, 2019-05-02 (annotated)
- Committer:
- shimizuta
- Date:
- Thu May 02 09:39:31 2019 +0000
- Revision:
- 18:d0a6771fa38d
- Parent:
- 17:2b3fa9b1a05b
- Child:
- 19:b162e7f4cc06
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
eri | 0:c1476d342c13 | 1 | #include "mbed.h" |
shimizuta | 17:2b3fa9b1a05b | 2 | #include "debug.h" |
eri | 0:c1476d342c13 | 3 | #include "pin.h" |
eri | 0:c1476d342c13 | 4 | #include "microinfinity.h" |
shimizuta | 17:2b3fa9b1a05b | 5 | #include "robot.h" |
shimizuta | 16:1e91753f0a01 | 6 | /* |
yuto17320508 | 4:81f01f93e502 | 7 | //#define DEBUG_ON |
yuto17320508 | 4:81f01f93e502 | 8 | |
yuto17320508 | 4:81f01f93e502 | 9 | #ifdef DEBUG_ON |
yuto17320508 | 4:81f01f93e502 | 10 | #define DEBUG(...) printf("" __VA_ARGS__); |
yuto17320508 | 4:81f01f93e502 | 11 | #else |
yuto17320508 | 4:81f01f93e502 | 12 | #define DEBUG(...) |
yuto17320508 | 4:81f01f93e502 | 13 | #endif |
shimizuta | 16:1e91753f0a01 | 14 | */ |
yuto17320508 | 4:81f01f93e502 | 15 | #define Pi 3.14159265359 //円周率π |
yuto17320508 | 4:81f01f93e502 | 16 | |
shimizuta | 18:d0a6771fa38d | 17 | //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う |
shimizuta | 18:d0a6771fa38d | 18 | PIDcontroller pid_lo(0.01, 0.000, 0.000); |
shimizuta | 18:d0a6771fa38d | 19 | PIDcontroller pid_li(0.01, 0.000, 0.000); //Kp.Ki,Kd |
shimizuta | 18:d0a6771fa38d | 20 | Motor motor_lo(&motor_lo_f, &motor_lo_b), |
shimizuta | 18:d0a6771fa38d | 21 | motor_li(&motor_li_f, &motor_li_b); //forward,backのピンを代入 |
shimizuta | 18:d0a6771fa38d | 22 | OneLeg leg_lo, leg_li; |
shimizuta | 18:d0a6771fa38d | 23 | Robot robot; |
shimizuta | 18:d0a6771fa38d | 24 | |
yuto17320508 | 14:1a3a673d85e3 | 25 | enum WalkMode { |
yuto17320508 | 12:9a5de6adae9c | 26 | BACK, |
yuto17320508 | 12:9a5de6adae9c | 27 | STOP, |
yuto17320508 | 12:9a5de6adae9c | 28 | FORWARD, |
yuto17320508 | 12:9a5de6adae9c | 29 | }; |
yuto17320508 | 14:1a3a673d85e3 | 30 | enum EVENT { |
yuto17320508 | 12:9a5de6adae9c | 31 | NORMAL, |
yuto17320508 | 12:9a5de6adae9c | 32 | GEREGE, |
yuto17320508 | 14:1a3a673d85e3 | 33 | GOAL, |
yuto17320508 | 12:9a5de6adae9c | 34 | }; |
yuto17320508 | 4:81f01f93e502 | 35 | float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。 |
shimizuta | 18:d0a6771fa38d | 36 | int hand_mode=NORMAL; |
shimizuta | 18:d0a6771fa38d | 37 | int step_num_l, step_num_r; |
yuto17320508 | 4:81f01f93e502 | 38 | |
yuto17320508 | 4:81f01f93e502 | 39 | void reset(); |
eri | 0:c1476d342c13 | 40 | void setup(); |
yuto17320508 | 10:a335588b9ef0 | 41 | void set_gyro(); |
yuto17320508 | 10:a335588b9ef0 | 42 | double get_dist_forward(); |
yuto17320508 | 10:a335588b9ef0 | 43 | double get_dist_back(); |
yuto17320508 | 12:9a5de6adae9c | 44 | void can_send(int mode, float duty); |
yuto17320508 | 12:9a5de6adae9c | 45 | void straight(); |
yuto17320508 | 12:9a5de6adae9c | 46 | void turnLeft(); |
yuto17320508 | 12:9a5de6adae9c | 47 | void curveLeft(); |
yuto17320508 | 12:9a5de6adae9c | 48 | void turnRight(); |
yuto17320508 | 12:9a5de6adae9c | 49 | void curveRight(); |
yuto17320508 | 13:29e71a2fd11e | 50 | void turn(float target_degree);//回転角, 収束許容誤差 |
yuto17320508 | 10:a335588b9ef0 | 51 | void straightAndInfinity(float target_degree, float tolerance_degree);//角度を補正するのと前進 |
yuto17320508 | 12:9a5de6adae9c | 52 | void wait_gerege(); |
yuto17320508 | 12:9a5de6adae9c | 53 | |
kageyuta | 1:86c4c38abe40 | 54 | int main() |
kageyuta | 1:86c4c38abe40 | 55 | { |
eri | 0:c1476d342c13 | 56 | setup(); |
yuto17320508 | 12:9a5de6adae9c | 57 | printf("reset standby\n\r"); |
kageyuta | 2:55c616d2e0fe | 58 | reset(); |
yuto17320508 | 4:81f01f93e502 | 59 | printf("bus standby\n\r"); |
yuto17320508 | 5:63462d696256 | 60 | while(1) { |
yuto17320508 | 5:63462d696256 | 61 | if(bus_in.read() == 1) break; |
yuto17320508 | 3:7a608fbd3bcd | 62 | } |
yuto17320508 | 4:81f01f93e502 | 63 | printf("bus is %d\n\r", bus_in.read()); |
yuto17320508 | 12:9a5de6adae9c | 64 | wait(0.5); |
yuto17320508 | 12:9a5de6adae9c | 65 | straight(); |
yuto17320508 | 12:9a5de6adae9c | 66 | wait_gerege(); |
yuto17320508 | 12:9a5de6adae9c | 67 | hand_mode = GEREGE; |
yuto17320508 | 10:a335588b9ef0 | 68 | set_gyro(); |
yuto17320508 | 10:a335588b9ef0 | 69 | printf("dist:%.3f\r\n", get_dist_forward()); |
shimizuta | 18:d0a6771fa38d | 70 | //直進スタート |
yuto17320508 | 13:29e71a2fd11e | 71 | for(int i=0;i<3;++i){ |
yuto17320508 | 13:29e71a2fd11e | 72 | while(get_dist_back() < 280) |
yuto17320508 | 13:29e71a2fd11e | 73 | { |
yuto17320508 | 13:29e71a2fd11e | 74 | straightAndInfinity(0, 5); |
yuto17320508 | 13:29e71a2fd11e | 75 | //wait(0.01); |
yuto17320508 | 14:1a3a673d85e3 | 76 | printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back()); |
yuto17320508 | 13:29e71a2fd11e | 77 | } |
yuto17320508 | 3:7a608fbd3bcd | 78 | } |
yuto17320508 | 12:9a5de6adae9c | 79 | //printf("back dist:%.3f\r\n", get_dist_back()); |
yuto17320508 | 10:a335588b9ef0 | 80 | //段差前旋回 |
yuto17320508 | 12:9a5de6adae9c | 81 | motor_lo.setDutyLimit(0.4);//0.5 |
yuto17320508 | 12:9a5de6adae9c | 82 | motor_li.setDutyLimit(0.4); |
yuto17320508 | 13:29e71a2fd11e | 83 | turn(40.0); |
yuto17320508 | 10:a335588b9ef0 | 84 | //段差乗り越え |
yuto17320508 | 12:9a5de6adae9c | 85 | for(int i= 0;i<5;++i) straight(); |
yuto17320508 | 12:9a5de6adae9c | 86 | while(get_dist_back() > 40) straight(); |
yuto17320508 | 10:a335588b9ef0 | 87 | //段差後旋回 |
yuto17320508 | 12:9a5de6adae9c | 88 | printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back()); |
yuto17320508 | 13:29e71a2fd11e | 89 | turn(90.0); |
yuto17320508 | 12:9a5de6adae9c | 90 | printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back()); |
yuto17320508 | 10:a335588b9ef0 | 91 | //前進 |
yuto17320508 | 12:9a5de6adae9c | 92 | motor_lo.setDutyLimit(0.5);//0.5 |
yuto17320508 | 12:9a5de6adae9c | 93 | motor_li.setDutyLimit(0.5); |
yuto17320508 | 13:29e71a2fd11e | 94 | for(int i=0;i<3;++i) |
yuto17320508 | 12:9a5de6adae9c | 95 | { |
yuto17320508 | 14:1a3a673d85e3 | 96 | while(get_dist_forward() > 65) |
yuto17320508 | 13:29e71a2fd11e | 97 | { |
yuto17320508 | 13:29e71a2fd11e | 98 | straightAndInfinity(90.0, 5.0); |
yuto17320508 | 13:29e71a2fd11e | 99 | printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back()); |
yuto17320508 | 13:29e71a2fd11e | 100 | } |
yuto17320508 | 12:9a5de6adae9c | 101 | } |
yuto17320508 | 10:a335588b9ef0 | 102 | //ロープ前旋回 |
yuto17320508 | 13:29e71a2fd11e | 103 | printf("before roop deg:%.3f\n\r",degree0); |
yuto17320508 | 13:29e71a2fd11e | 104 | turn(0); |
yuto17320508 | 14:1a3a673d85e3 | 105 | |
yuto17320508 | 10:a335588b9ef0 | 106 | //ロープ |
yuto17320508 | 13:29e71a2fd11e | 107 | while(mode4.read() == 0) |
yuto17320508 | 13:29e71a2fd11e | 108 | { |
yuto17320508 | 14:1a3a673d85e3 | 109 | |
yuto17320508 | 13:29e71a2fd11e | 110 | straightAndInfinity(0, 5); |
yuto17320508 | 13:29e71a2fd11e | 111 | } |
yuto17320508 | 14:1a3a673d85e3 | 112 | |
yuto17320508 | 14:1a3a673d85e3 | 113 | printf("go to uhai deg:%.3f forward dist:%.3f \n\r",degree0,get_dist_forward()); |
yuto17320508 | 13:29e71a2fd11e | 114 | for(int i=0;i<3;++i) |
kageyuta | 11:a6fadff7cc78 | 115 | { |
yuto17320508 | 14:1a3a673d85e3 | 116 | while(get_dist_forward() > 65)//65 |
yuto17320508 | 13:29e71a2fd11e | 117 | { |
yuto17320508 | 13:29e71a2fd11e | 118 | straightAndInfinity(0, 5); |
yuto17320508 | 13:29e71a2fd11e | 119 | printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back()); |
yuto17320508 | 13:29e71a2fd11e | 120 | } |
kageyuta | 11:a6fadff7cc78 | 121 | } |
yuto17320508 | 14:1a3a673d85e3 | 122 | printf("turn"); |
yuto17320508 | 14:1a3a673d85e3 | 123 | turn(-90); |
yuto17320508 | 14:1a3a673d85e3 | 124 | |
yuto17320508 | 15:1098bf926b5b | 125 | while(get_dist_back() > 10.0){} |
yuto17320508 | 15:1098bf926b5b | 126 | while(get_dist_back() < 30.0){} |
yuto17320508 | 15:1098bf926b5b | 127 | |
yuto17320508 | 15:1098bf926b5b | 128 | printf("last spart!!!!!!!!"); |
yuto17320508 | 12:9a5de6adae9c | 129 | |
yuto17320508 | 14:1a3a673d85e3 | 130 | motor_lo.setDutyLimit(0.2);//0.5 |
yuto17320508 | 14:1a3a673d85e3 | 131 | motor_li.setDutyLimit(0.2); |
yuto17320508 | 14:1a3a673d85e3 | 132 | |
yuto17320508 | 14:1a3a673d85e3 | 133 | for(int i=0; i<15; ++i)straightAndInfinity(0, 5); |
yuto17320508 | 14:1a3a673d85e3 | 134 | printf("wall standby"); |
yuto17320508 | 14:1a3a673d85e3 | 135 | while(get_dist_back() < 250) { |
yuto17320508 | 14:1a3a673d85e3 | 136 | straightAndInfinity(0, 5); |
yuto17320508 | 14:1a3a673d85e3 | 137 | printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back()); |
yuto17320508 | 14:1a3a673d85e3 | 138 | } |
yuto17320508 | 14:1a3a673d85e3 | 139 | for(int i=0; i<2; ++i) { |
yuto17320508 | 14:1a3a673d85e3 | 140 | while(get_dist_forward() > 65) { |
yuto17320508 | 14:1a3a673d85e3 | 141 | straightAndInfinity(0, 5); |
yuto17320508 | 14:1a3a673d85e3 | 142 | printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back()); |
yuto17320508 | 14:1a3a673d85e3 | 143 | } |
yuto17320508 | 14:1a3a673d85e3 | 144 | } |
yuto17320508 | 12:9a5de6adae9c | 145 | hand_mode = GOAL; |
yuto17320508 | 12:9a5de6adae9c | 146 | straight(); |
yuto17320508 | 13:29e71a2fd11e | 147 | printf("uhai!!!!!!!!!!!!!!!"); |
yuto17320508 | 10:a335588b9ef0 | 148 | } |
yuto17320508 | 13:29e71a2fd11e | 149 | void turn(float target_degree)//turn_degreeを超えるまで旋回し続ける関数 |
yuto17320508 | 10:a335588b9ef0 | 150 | { |
yuto17320508 | 14:1a3a673d85e3 | 151 | if(target_degree - degree0 > 0) { |
yuto17320508 | 10:a335588b9ef0 | 152 | while(target_degree - degree0 > 0) |
yuto17320508 | 14:1a3a673d85e3 | 153 | turnLeft(); |
yuto17320508 | 14:1a3a673d85e3 | 154 | } else { |
yuto17320508 | 10:a335588b9ef0 | 155 | while(target_degree - degree0 < 0) |
yuto17320508 | 14:1a3a673d85e3 | 156 | turnRight(); |
yuto17320508 | 10:a335588b9ef0 | 157 | } |
yuto17320508 | 13:29e71a2fd11e | 158 | printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back()); |
yuto17320508 | 10:a335588b9ef0 | 159 | } |
yuto17320508 | 10:a335588b9ef0 | 160 | void straightAndInfinity(float target_degree, float tolerance_degree) |
yuto17320508 | 10:a335588b9ef0 | 161 | { |
yuto17320508 | 12:9a5de6adae9c | 162 | if(target_degree - degree0 > tolerance_degree) curveLeft(); |
yuto17320508 | 12:9a5de6adae9c | 163 | else if(degree0 - target_degree > tolerance_degree) curveRight(); |
yuto17320508 | 12:9a5de6adae9c | 164 | else straight(); |
yuto17320508 | 5:63462d696256 | 165 | } |
yuto17320508 | 12:9a5de6adae9c | 166 | void straight() |
yuto17320508 | 5:63462d696256 | 167 | { |
yuto17320508 | 12:9a5de6adae9c | 168 | can_send(FORWARD, motor_lo.getDutyLimit()); |
yuto17320508 | 5:63462d696256 | 169 | leg_lo.setTargetPose(360+step_num_l*180); |
yuto17320508 | 5:63462d696256 | 170 | leg_li.setTargetPose(180+step_num_l*180); |
yuto17320508 | 5:63462d696256 | 171 | robot.run(); |
yuto17320508 | 4:81f01f93e502 | 172 | motor_lo_f.write(0); |
yuto17320508 | 4:81f01f93e502 | 173 | motor_lo_b.write(0); |
yuto17320508 | 4:81f01f93e502 | 174 | motor_li_f.write(0); |
yuto17320508 | 4:81f01f93e502 | 175 | motor_li_b.write(0); |
yuto17320508 | 5:63462d696256 | 176 | while(1) { |
yuto17320508 | 5:63462d696256 | 177 | if(bus_in.read() == 1) break; |
yuto17320508 | 5:63462d696256 | 178 | } |
yuto17320508 | 7:cff25545558f | 179 | step_num_l++; |
yuto17320508 | 7:cff25545558f | 180 | step_num_r++; |
yuto17320508 | 5:63462d696256 | 181 | } |
yuto17320508 | 12:9a5de6adae9c | 182 | void turnLeft() |
yuto17320508 | 5:63462d696256 | 183 | { |
yuto17320508 | 12:9a5de6adae9c | 184 | can_send(FORWARD, motor_lo.getDutyLimit()); |
yuto17320508 | 5:63462d696256 | 185 | leg_lo.setTargetPose(360+(step_num_l-2)*180); |
yuto17320508 | 5:63462d696256 | 186 | leg_li.setTargetPose(180+(step_num_l-2)*180); |
yuto17320508 | 5:63462d696256 | 187 | robot.run(); |
yuto17320508 | 5:63462d696256 | 188 | motor_lo_f.write(0); |
yuto17320508 | 5:63462d696256 | 189 | motor_lo_b.write(0); |
yuto17320508 | 5:63462d696256 | 190 | motor_li_f.write(0); |
yuto17320508 | 5:63462d696256 | 191 | motor_li_b.write(0); |
yuto17320508 | 5:63462d696256 | 192 | while(1) { |
yuto17320508 | 5:63462d696256 | 193 | if(bus_in.read() == 1) break; |
yuto17320508 | 5:63462d696256 | 194 | } |
yuto17320508 | 5:63462d696256 | 195 | step_num_r++; |
yuto17320508 | 5:63462d696256 | 196 | step_num_l--; |
yuto17320508 | 5:63462d696256 | 197 | } |
yuto17320508 | 12:9a5de6adae9c | 198 | void curveLeft() |
yuto17320508 | 10:a335588b9ef0 | 199 | { |
yuto17320508 | 12:9a5de6adae9c | 200 | can_send(FORWARD, motor_lo.getDutyLimit()); |
yuto17320508 | 10:a335588b9ef0 | 201 | leg_lo.setTargetPose(360+(step_num_l-1)*180); |
yuto17320508 | 10:a335588b9ef0 | 202 | leg_li.setTargetPose(180+(step_num_l-1)*180); |
yuto17320508 | 10:a335588b9ef0 | 203 | robot.run(); |
yuto17320508 | 10:a335588b9ef0 | 204 | motor_lo_f.write(0); |
yuto17320508 | 10:a335588b9ef0 | 205 | motor_lo_b.write(0); |
yuto17320508 | 10:a335588b9ef0 | 206 | motor_li_f.write(0); |
yuto17320508 | 10:a335588b9ef0 | 207 | motor_li_b.write(0); |
yuto17320508 | 10:a335588b9ef0 | 208 | while(1) { |
yuto17320508 | 10:a335588b9ef0 | 209 | if(bus_in.read() == 1) break; |
yuto17320508 | 10:a335588b9ef0 | 210 | } |
yuto17320508 | 10:a335588b9ef0 | 211 | step_num_r++; |
yuto17320508 | 10:a335588b9ef0 | 212 | } |
yuto17320508 | 12:9a5de6adae9c | 213 | void turnRight() |
yuto17320508 | 5:63462d696256 | 214 | { |
yuto17320508 | 12:9a5de6adae9c | 215 | can_send(BACK, motor_lo.getDutyLimit()); |
yuto17320508 | 8:ded0354412ae | 216 | leg_lo.setTargetPose(360+step_num_l*180); |
yuto17320508 | 8:ded0354412ae | 217 | leg_li.setTargetPose(180+step_num_l*180); |
yuto17320508 | 5:63462d696256 | 218 | robot.run(); |
yuto17320508 | 5:63462d696256 | 219 | motor_lo_f.write(0); |
yuto17320508 | 5:63462d696256 | 220 | motor_lo_b.write(0); |
yuto17320508 | 5:63462d696256 | 221 | motor_li_f.write(0); |
yuto17320508 | 5:63462d696256 | 222 | motor_li_b.write(0); |
yuto17320508 | 5:63462d696256 | 223 | while(1) { |
yuto17320508 | 5:63462d696256 | 224 | if(bus_in.read() == 1) break; |
yuto17320508 | 5:63462d696256 | 225 | } |
yuto17320508 | 8:ded0354412ae | 226 | step_num_r--; |
yuto17320508 | 8:ded0354412ae | 227 | step_num_l++; |
eri | 0:c1476d342c13 | 228 | } |
yuto17320508 | 12:9a5de6adae9c | 229 | void curveRight() |
yuto17320508 | 10:a335588b9ef0 | 230 | { |
yuto17320508 | 12:9a5de6adae9c | 231 | can_send(STOP, motor_lo.getDutyLimit()); |
yuto17320508 | 10:a335588b9ef0 | 232 | leg_lo.setTargetPose(360+step_num_l*180); |
yuto17320508 | 10:a335588b9ef0 | 233 | leg_li.setTargetPose(180+step_num_l*180); |
yuto17320508 | 10:a335588b9ef0 | 234 | robot.run(); |
yuto17320508 | 10:a335588b9ef0 | 235 | motor_lo_f.write(0); |
yuto17320508 | 10:a335588b9ef0 | 236 | motor_lo_b.write(0); |
yuto17320508 | 10:a335588b9ef0 | 237 | motor_li_f.write(0); |
yuto17320508 | 10:a335588b9ef0 | 238 | motor_li_b.write(0); |
yuto17320508 | 10:a335588b9ef0 | 239 | while(1) { |
yuto17320508 | 10:a335588b9ef0 | 240 | if(bus_in.read() == 1) break; |
yuto17320508 | 10:a335588b9ef0 | 241 | } |
yuto17320508 | 10:a335588b9ef0 | 242 | step_num_l++; |
yuto17320508 | 10:a335588b9ef0 | 243 | } |
eri | 0:c1476d342c13 | 244 | |
eri | 0:c1476d342c13 | 245 | |
kageyuta | 1:86c4c38abe40 | 246 | void setup() |
kageyuta | 1:86c4c38abe40 | 247 | { |
eri | 0:c1476d342c13 | 248 | can1.frequency(1000000); |
eri | 0:c1476d342c13 | 249 | motor_lo_f.period_us(100); |
eri | 0:c1476d342c13 | 250 | motor_lo_b.period_us(100); |
eri | 0:c1476d342c13 | 251 | motor_li_f.period_us(100); |
eri | 0:c1476d342c13 | 252 | motor_li_b.period_us(100); |
kageyuta | 1:86c4c38abe40 | 253 | |
eri | 0:c1476d342c13 | 254 | hand.mode(PullUp); |
kageyuta | 2:55c616d2e0fe | 255 | switch_lo.mode(PullUp); |
kageyuta | 2:55c616d2e0fe | 256 | switch_li.mode(PullUp); |
yuto17320508 | 13:29e71a2fd11e | 257 | mode4.mode(PullUp); |
shimizuta | 18:d0a6771fa38d | 258 | |
shimizuta | 18:d0a6771fa38d | 259 | pid_lo.setTolerance(10); |
shimizuta | 18:d0a6771fa38d | 260 | pid_li.setTolerance(10); |
shimizuta | 18:d0a6771fa38d | 261 | motor_lo.setEncoder(&ec_lo); |
shimizuta | 18:d0a6771fa38d | 262 | motor_lo.setResolution(1000); |
shimizuta | 18:d0a6771fa38d | 263 | motor_li.setEncoder(&ec_li); |
shimizuta | 18:d0a6771fa38d | 264 | motor_li.setResolution(600); |
shimizuta | 18:d0a6771fa38d | 265 | leg_lo.setMotor(&motor_lo); |
shimizuta | 18:d0a6771fa38d | 266 | leg_lo.setPIDcontroller(&pid_lo); |
shimizuta | 18:d0a6771fa38d | 267 | leg_li.setMotor(&motor_li); |
shimizuta | 18:d0a6771fa38d | 268 | leg_li.setPIDcontroller(&pid_li); |
shimizuta | 18:d0a6771fa38d | 269 | robot.setLeg(&leg_lo, &leg_li); |
shimizuta | 18:d0a6771fa38d | 270 | robot.setTickerTime(0.01); //モータ出力間隔 0.01 |
shimizuta | 18:d0a6771fa38d | 271 | motor_lo.setDutyLimit(0.5);//0.5 |
shimizuta | 18:d0a6771fa38d | 272 | motor_li.setDutyLimit(0.5); |
kageyuta | 1:86c4c38abe40 | 273 | |
yuto17320508 | 10:a335588b9ef0 | 274 | } |
eri | 0:c1476d342c13 | 275 | |
yuto17320508 | 10:a335588b9ef0 | 276 | void set_gyro() |
yuto17320508 | 10:a335588b9ef0 | 277 | { |
eri | 0:c1476d342c13 | 278 | device.baud(115200); |
eri | 0:c1476d342c13 | 279 | device.format(8,Serial::None,1); |
eri | 0:c1476d342c13 | 280 | device.attach(dev_rx, Serial::RxIrq); |
eri | 0:c1476d342c13 | 281 | wait(0.05); |
eri | 0:c1476d342c13 | 282 | theta0=degree0; |
eri | 0:c1476d342c13 | 283 | check_gyro(); |
eri | 0:c1476d342c13 | 284 | } |
eri | 0:c1476d342c13 | 285 | |
eri | 0:c1476d342c13 | 286 | |
eri | 0:c1476d342c13 | 287 | //////////////////////////////////////can |
yuto17320508 | 12:9a5de6adae9c | 288 | void can_send(int mode, float duty) |
eri | 0:c1476d342c13 | 289 | { |
yuto17320508 | 12:9a5de6adae9c | 290 | char data[2]= {0}; |
yuto17320508 | 12:9a5de6adae9c | 291 | int send=mode * 10 + (int)(duty * 10.0); |
yuto17320508 | 12:9a5de6adae9c | 292 | //printf("duty is %.3f\n\r",duty); |
yuto17320508 | 12:9a5de6adae9c | 293 | data[0]=send & 0b11111111; |
yuto17320508 | 12:9a5de6adae9c | 294 | data[1]=hand_mode; |
kageyuta | 1:86c4c38abe40 | 295 | |
yuto17320508 | 12:9a5de6adae9c | 296 | if(can1.write(CANMessage(0,data,2)))led4=1; |
eri | 0:c1476d342c13 | 297 | else led4=0; |
kageyuta | 1:86c4c38abe40 | 298 | } |
kageyuta | 2:55c616d2e0fe | 299 | void reset() |
kageyuta | 2:55c616d2e0fe | 300 | { |
kageyuta | 2:55c616d2e0fe | 301 | while(switch_lo.read()) { |
kageyuta | 6:fe9fa8c2e6f9 | 302 | motor_lo.output(0.3); |
kageyuta | 2:55c616d2e0fe | 303 | } |
yuto17320508 | 5:63462d696256 | 304 | ec_lo.reset(); |
yuto17320508 | 5:63462d696256 | 305 | motor_lo.output(0.0); |
kageyuta | 2:55c616d2e0fe | 306 | while(switch_li.read()) { |
kageyuta | 6:fe9fa8c2e6f9 | 307 | motor_li.output(0.3); |
kageyuta | 2:55c616d2e0fe | 308 | } |
yuto17320508 | 5:63462d696256 | 309 | ec_li.reset(); |
yuto17320508 | 5:63462d696256 | 310 | motor_li.output(0.0); |
kageyuta | 1:86c4c38abe40 | 311 | } |
yuto17320508 | 10:a335588b9ef0 | 312 | double get_dist_back() |
yuto17320508 | 10:a335588b9ef0 | 313 | { |
yuto17320508 | 10:a335588b9ef0 | 314 | sensor_back.start(); |
yuto17320508 | 14:1a3a673d85e3 | 315 | wait_ms(10);//10 //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。 |
yuto17320508 | 10:a335588b9ef0 | 316 | //何ループも回す場合は不要。また、時間は適当だから調整が必要。 |
yuto17320508 | 14:1a3a673d85e3 | 317 | float dist = sensor_back.get_dist_cm(); |
yuto17320508 | 14:1a3a673d85e3 | 318 | if(dist < 0.1) dist = 2000.0; |
yuto17320508 | 14:1a3a673d85e3 | 319 | return dist; |
yuto17320508 | 10:a335588b9ef0 | 320 | } |
yuto17320508 | 10:a335588b9ef0 | 321 | double get_dist_forward() |
yuto17320508 | 10:a335588b9ef0 | 322 | { |
kageyuta | 11:a6fadff7cc78 | 323 | sensor_forward.start(); |
yuto17320508 | 14:1a3a673d85e3 | 324 | wait_ms(10);//10 |
yuto17320508 | 14:1a3a673d85e3 | 325 | float dist = sensor_forward.get_dist_cm(); |
yuto17320508 | 14:1a3a673d85e3 | 326 | if(dist < 0.1) dist = 2000.0; |
yuto17320508 | 14:1a3a673d85e3 | 327 | return dist; |
yuto17320508 | 10:a335588b9ef0 | 328 | } |
yuto17320508 | 12:9a5de6adae9c | 329 | |
yuto17320508 | 12:9a5de6adae9c | 330 | void wait_gerege() |
yuto17320508 | 12:9a5de6adae9c | 331 | { |
yuto17320508 | 12:9a5de6adae9c | 332 | int i = 0; |
yuto17320508 | 14:1a3a673d85e3 | 333 | while(i!=100) { |
yuto17320508 | 14:1a3a673d85e3 | 334 | if(hand.read()==0)i++; |
yuto17320508 | 14:1a3a673d85e3 | 335 | } |
yuto17320508 | 12:9a5de6adae9c | 336 | } |