yuto kawamura
/
George_Slave_BOTHMOVE
right and left move at the same time
main.cpp@10:2973cea54efd, 2019-05-08 (annotated)
- Committer:
- yuto17320508
- Date:
- Wed May 08 08:21:08 2019 +0000
- Revision:
- 10:2973cea54efd
- Parent:
- 9:ac95473a5d86
- Child:
- 11:0522b336eb82
l
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
eri | 0:411ab20ce87d | 1 | #include "mbed.h" |
eri | 0:411ab20ce87d | 2 | #include "pin.h" |
shimizuta | 9:ac95473a5d86 | 3 | #include "debug.h" |
shimizuta | 9:ac95473a5d86 | 4 | #include "robot.h" |
shimizuta | 9:ac95473a5d86 | 5 | enum WALKMODE { |
yuto17320508 | 7:c5c60192eb02 | 6 | BACK, |
yuto17320508 | 7:c5c60192eb02 | 7 | STOP, |
yuto17320508 | 7:c5c60192eb02 | 8 | FORWARD, |
yuto17320508 | 7:c5c60192eb02 | 9 | }; |
shimizuta | 9:ac95473a5d86 | 10 | enum EVENT { |
yuto17320508 | 7:c5c60192eb02 | 11 | NORMAL, |
yuto17320508 | 7:c5c60192eb02 | 12 | GEREGE, |
shimizuta | 9:ac95473a5d86 | 13 | GOAL, |
yuto17320508 | 7:c5c60192eb02 | 14 | }; |
kageyuta | 1:34371ffd3dc0 | 15 | |
yuto17320508 | 2:db2bc2ae4d20 | 16 | float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。 |
yuto17320508 | 8:43c31d2afb9e | 17 | int hand_mode=NORMAL; |
yuto17320508 | 2:db2bc2ae4d20 | 18 | |
yuto17320508 | 2:db2bc2ae4d20 | 19 | void setup(); |
yuto17320508 | 7:c5c60192eb02 | 20 | void forward(); |
yuto17320508 | 7:c5c60192eb02 | 21 | void stop(); |
yuto17320508 | 7:c5c60192eb02 | 22 | void back(); |
yuto17320508 | 7:c5c60192eb02 | 23 | void can_receive(int &mode, float &duty); |
yuto17320508 | 2:db2bc2ae4d20 | 24 | void reset(); |
eri | 0:411ab20ce87d | 25 | |
yuto17320508 | 7:c5c60192eb02 | 26 | |
yuto17320508 | 2:db2bc2ae4d20 | 27 | //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う |
yuto17320508 | 2:db2bc2ae4d20 | 28 | //しかし変更を多々行うためポインタ渡しにしてある |
yuto17320508 | 2:db2bc2ae4d20 | 29 | //文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う |
yuto17320508 | 2:db2bc2ae4d20 | 30 | PIDcontroller pid_ro(0.01, 0.000, 0.000); |
yuto17320508 | 2:db2bc2ae4d20 | 31 | PIDcontroller pid_ri(0.01, 0.000, 0.000); //Kp.Ki,Kd |
yuto17320508 | 2:db2bc2ae4d20 | 32 | Motor motor_ro(&motor_ro_f, &motor_ro_b), |
yuto17320508 | 7:c5c60192eb02 | 33 | motor_ri(&motor_ri_f, &motor_ri_b); //forward,backのピンを代入 |
yuto17320508 | 2:db2bc2ae4d20 | 34 | OneLeg leg_ro, leg_ri; |
yuto17320508 | 2:db2bc2ae4d20 | 35 | Robot robot; |
eri | 0:411ab20ce87d | 36 | |
yuto17320508 | 7:c5c60192eb02 | 37 | int step_num_r = 0; |
kageyuta | 1:34371ffd3dc0 | 38 | int main() |
kageyuta | 1:34371ffd3dc0 | 39 | { |
yuto17320508 | 2:db2bc2ae4d20 | 40 | printf("standby ok\n\r"); |
eri | 0:411ab20ce87d | 41 | setup(); |
yuto17320508 | 2:db2bc2ae4d20 | 42 | pid_ro.setTolerance(10); |
yuto17320508 | 2:db2bc2ae4d20 | 43 | pid_ri.setTolerance(10); |
yuto17320508 | 7:c5c60192eb02 | 44 | |
yuto17320508 | 2:db2bc2ae4d20 | 45 | motor_ro.setEncoder(&ec_ro); |
yuto17320508 | 2:db2bc2ae4d20 | 46 | motor_ro.setResolution(1000); |
yuto17320508 | 2:db2bc2ae4d20 | 47 | motor_ri.setEncoder(&ec_ri); |
yuto17320508 | 2:db2bc2ae4d20 | 48 | motor_ri.setResolution(1000); |
yuto17320508 | 7:c5c60192eb02 | 49 | |
yuto17320508 | 2:db2bc2ae4d20 | 50 | leg_ro.setMotor(&motor_ro); |
yuto17320508 | 2:db2bc2ae4d20 | 51 | leg_ro.setPIDcontroller(&pid_ro); |
yuto17320508 | 2:db2bc2ae4d20 | 52 | leg_ri.setMotor(&motor_ri); |
yuto17320508 | 2:db2bc2ae4d20 | 53 | leg_ri.setPIDcontroller(&pid_ri); |
yuto17320508 | 7:c5c60192eb02 | 54 | |
yuto17320508 | 2:db2bc2ae4d20 | 55 | robot.setLeg(&leg_ro, &leg_ri); |
yuto17320508 | 2:db2bc2ae4d20 | 56 | robot.setTickerTime(0.01); //モータ出力間隔 0.01 |
yuto17320508 | 7:c5c60192eb02 | 57 | |
yuto17320508 | 6:2bc2950f32d8 | 58 | motor_ro.setDutyLimit(0.5); |
yuto17320508 | 6:2bc2950f32d8 | 59 | motor_ri.setDutyLimit(0.5); |
yuto17320508 | 7:c5c60192eb02 | 60 | |
kageyuta | 1:34371ffd3dc0 | 61 | reset(); |
yuto17320508 | 10:2973cea54efd | 62 | for(int i=0; i<5; ++i) |
yuto17320508 | 10:2973cea54efd | 63 | servo.set_degree(0,0); |
yuto17320508 | 2:db2bc2ae4d20 | 64 | bus_out = 1; |
yuto17320508 | 2:db2bc2ae4d20 | 65 | printf("start\n\r"); |
yuto17320508 | 7:c5c60192eb02 | 66 | int mode = -1; |
yuto17320508 | 7:c5c60192eb02 | 67 | air_hand = 1; |
shimizuta | 9:ac95473a5d86 | 68 | |
shimizuta | 9:ac95473a5d86 | 69 | while(1) { |
yuto17320508 | 7:c5c60192eb02 | 70 | float duty = 0.0; |
yuto17320508 | 7:c5c60192eb02 | 71 | can_receive(mode,duty); |
yuto17320508 | 7:c5c60192eb02 | 72 | printf("mode:%d duty:%.3f\n\r", mode, duty); |
yuto17320508 | 7:c5c60192eb02 | 73 | motor_ro.setDutyLimit(duty); |
yuto17320508 | 7:c5c60192eb02 | 74 | motor_ri.setDutyLimit(duty); |
shimizuta | 9:ac95473a5d86 | 75 | if(hand_mode == GEREGE) { |
yuto17320508 | 7:c5c60192eb02 | 76 | air_hand = 0; |
yuto17320508 | 10:2973cea54efd | 77 | |
shimizuta | 9:ac95473a5d86 | 78 | } else if(hand_mode == GOAL) { |
shimizuta | 9:ac95473a5d86 | 79 | for(int i=0; i<10; ++i) |
shimizuta | 9:ac95473a5d86 | 80 | servo.set_degree(0,(7200 - 3500) * 270.0/(11500 - 3500)); |
yuto17320508 | 7:c5c60192eb02 | 81 | } |
yuto17320508 | 7:c5c60192eb02 | 82 | bus_out = 0; |
yuto17320508 | 7:c5c60192eb02 | 83 | //printf("target is %d\n\r",target_ro); |
yuto17320508 | 7:c5c60192eb02 | 84 | if(mode == FORWARD) forward(); |
yuto17320508 | 7:c5c60192eb02 | 85 | else if(mode == STOP) stop(); |
yuto17320508 | 7:c5c60192eb02 | 86 | else if(mode == BACK) back(); |
yuto17320508 | 2:db2bc2ae4d20 | 87 | bus_out = 1; |
eri | 0:411ab20ce87d | 88 | } |
yuto17320508 | 7:c5c60192eb02 | 89 | |
eri | 0:411ab20ce87d | 90 | } |
eri | 0:411ab20ce87d | 91 | |
yuto17320508 | 7:c5c60192eb02 | 92 | void forward() |
yuto17320508 | 4:db1640bd0e89 | 93 | { |
yuto17320508 | 7:c5c60192eb02 | 94 | leg_ro.setTargetPose(360+step_num_r*180); |
yuto17320508 | 7:c5c60192eb02 | 95 | leg_ri.setTargetPose(180+step_num_r*180); |
yuto17320508 | 7:c5c60192eb02 | 96 | robot.run(); |
yuto17320508 | 7:c5c60192eb02 | 97 | motor_ro_f.write(0); |
yuto17320508 | 7:c5c60192eb02 | 98 | motor_ro_b.write(0); |
yuto17320508 | 7:c5c60192eb02 | 99 | motor_ri_f.write(0); |
yuto17320508 | 7:c5c60192eb02 | 100 | motor_ri_b.write(0); |
yuto17320508 | 7:c5c60192eb02 | 101 | step_num_r++; |
yuto17320508 | 7:c5c60192eb02 | 102 | } |
yuto17320508 | 7:c5c60192eb02 | 103 | void stop() |
yuto17320508 | 7:c5c60192eb02 | 104 | { |
yuto17320508 | 7:c5c60192eb02 | 105 | leg_ro.setTargetPose(360+(step_num_r-1)*180); |
yuto17320508 | 7:c5c60192eb02 | 106 | leg_ri.setTargetPose(180+(step_num_r-1)*180); |
yuto17320508 | 4:db1640bd0e89 | 107 | robot.run(); |
yuto17320508 | 4:db1640bd0e89 | 108 | motor_ro_f.write(0); |
yuto17320508 | 4:db1640bd0e89 | 109 | motor_ro_b.write(0); |
yuto17320508 | 4:db1640bd0e89 | 110 | motor_ri_f.write(0); |
yuto17320508 | 4:db1640bd0e89 | 111 | motor_ri_b.write(0); |
yuto17320508 | 4:db1640bd0e89 | 112 | } |
yuto17320508 | 7:c5c60192eb02 | 113 | |
yuto17320508 | 7:c5c60192eb02 | 114 | void back() |
yuto17320508 | 4:db1640bd0e89 | 115 | { |
yuto17320508 | 7:c5c60192eb02 | 116 | leg_ro.setTargetPose(360+(step_num_r-2)*180); |
yuto17320508 | 7:c5c60192eb02 | 117 | leg_ri.setTargetPose(180+(step_num_r-2)*180); |
yuto17320508 | 4:db1640bd0e89 | 118 | robot.run(); |
yuto17320508 | 4:db1640bd0e89 | 119 | motor_ro_f.write(0); |
yuto17320508 | 4:db1640bd0e89 | 120 | motor_ro_b.write(0); |
yuto17320508 | 4:db1640bd0e89 | 121 | motor_ri_f.write(0); |
yuto17320508 | 4:db1640bd0e89 | 122 | motor_ri_b.write(0); |
yuto17320508 | 7:c5c60192eb02 | 123 | step_num_r--; |
yuto17320508 | 4:db1640bd0e89 | 124 | } |
eri | 0:411ab20ce87d | 125 | ////////////////////////////////////////////// |
kageyuta | 1:34371ffd3dc0 | 126 | void setup() |
kageyuta | 1:34371ffd3dc0 | 127 | { |
eri | 0:411ab20ce87d | 128 | can.frequency(1000000); |
eri | 0:411ab20ce87d | 129 | motor_ro_f.period_us(100); |
eri | 0:411ab20ce87d | 130 | motor_ro_b.period_us(100); |
eri | 0:411ab20ce87d | 131 | motor_ri_f.period_us(100); |
eri | 0:411ab20ce87d | 132 | motor_ri_b.period_us(100); |
shimizuta | 9:ac95473a5d86 | 133 | air_hand = 1;//ゲレゲを離す手の形にしておく |
kageyuta | 1:34371ffd3dc0 | 134 | switch_ro.mode(PullUp); |
kageyuta | 1:34371ffd3dc0 | 135 | switch_ri.mode(PullUp); |
eri | 0:411ab20ce87d | 136 | servo.init(); |
yuto17320508 | 7:c5c60192eb02 | 137 | wait(0.01); |
shimizuta | 9:ac95473a5d86 | 138 | for(int i=0; i<5; ++i) |
yuto17320508 | 7:c5c60192eb02 | 139 | servo.set_degree(0,0); |
yuto17320508 | 7:c5c60192eb02 | 140 | //(7200 - 3500) * 270.0/(11500 - 3500) |
eri | 0:411ab20ce87d | 141 | } |
eri | 0:411ab20ce87d | 142 | |
kageyuta | 1:34371ffd3dc0 | 143 | void reset() |
kageyuta | 1:34371ffd3dc0 | 144 | { |
kageyuta | 1:34371ffd3dc0 | 145 | while(switch_ro.read()) { |
kageyuta | 3:29999b02e940 | 146 | motor_ro.output(0.3); |
kageyuta | 1:34371ffd3dc0 | 147 | } |
yuto17320508 | 7:c5c60192eb02 | 148 | ec_ro.reset(); |
yuto17320508 | 7:c5c60192eb02 | 149 | motor_ro.output(0.0); |
yuto17320508 | 7:c5c60192eb02 | 150 | printf("ro OK\n\r"); |
kageyuta | 3:29999b02e940 | 151 | while(switch_ri.read()) { |
kageyuta | 3:29999b02e940 | 152 | motor_ri.output(0.3); |
kageyuta | 3:29999b02e940 | 153 | } |
yuto17320508 | 7:c5c60192eb02 | 154 | |
yuto17320508 | 7:c5c60192eb02 | 155 | ec_ri.reset(); |
yuto17320508 | 7:c5c60192eb02 | 156 | motor_ri.output(0.0); |
yuto17320508 | 7:c5c60192eb02 | 157 | printf("ri OK\n\r"); |
eri | 0:411ab20ce87d | 158 | } |
eri | 0:411ab20ce87d | 159 | |
yuto17320508 | 7:c5c60192eb02 | 160 | void can_receive(int &mode, float &duty) |
kageyuta | 1:34371ffd3dc0 | 161 | { |
eri | 0:411ab20ce87d | 162 | CANMessage msg; |
yuto17320508 | 7:c5c60192eb02 | 163 | while(1) { |
yuto17320508 | 7:c5c60192eb02 | 164 | //printf("roop\r\n"); |
eri | 0:411ab20ce87d | 165 | if(can.read(msg)) { |
eri | 0:411ab20ce87d | 166 | if(msg.id==0) { |
yuto17320508 | 7:c5c60192eb02 | 167 | mode= msg.data[0]*0.1; |
yuto17320508 | 7:c5c60192eb02 | 168 | duty = (msg.data[0]%10)*0.1; |
yuto17320508 | 7:c5c60192eb02 | 169 | hand_mode= msg.data[1]; |
eri | 0:411ab20ce87d | 170 | break; |
eri | 0:411ab20ce87d | 171 | } |
kageyuta | 1:34371ffd3dc0 | 172 | led2=1; |
eri | 0:411ab20ce87d | 173 | } else led2=0; |
eri | 0:411ab20ce87d | 174 | } |
yuto17320508 | 7:c5c60192eb02 | 175 | } |