yuto kawamura / Mbed 2 deprecated George_Slave_BOTHMOVE

right and left move at the same time

Dependencies:   mbed robot

main.cpp@13:678870d8f851, 2019-05-15 (annotated)

Committer:
yuto17320508
Date:
Wed May 15 10:19:13 2019 +0000
Revision:
13:678870d8f851
Parent:
12:e6fd3a3201f0 
l

Who changed what in which revision?

UserRevisionLine numberNew 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 12:e6fd3a3201f0 11 G_OPEN,
yuto17320508 12:e6fd3a3201f0 12 G_CLOSE,
shimizuta 9:ac95473a5d86 13 GOAL,
yuto17320508 7:c5c60192eb02 14 };
kageyuta 1:34371ffd3dc0 15
yuto17320508 2:db2bc2ae4d20 16 float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
yuto17320508 12:e6fd3a3201f0 17 int hand_mode=G_CLOSE;
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;
shimizuta 11:0522b336eb82 36 DigitalOut led[] = {LED1,LED2,LED3,LED4};
shimizuta 11:0522b336eb82 37
eri 0:411ab20ce87d 38
yuto17320508 7:c5c60192eb02 39 int step_num_r = 0;
kageyuta 1:34371ffd3dc0 40 int main()
kageyuta 1:34371ffd3dc0 41 {
yuto17320508 2:db2bc2ae4d20 42 printf("standby ok\n\r");
eri 0:411ab20ce87d 43 setup();
yuto17320508 12:e6fd3a3201f0 44
yuto17320508 12:e6fd3a3201f0 45 /*while(1){
yuto17320508 12:e6fd3a3201f0 46 servo.set_degree(0,(7200 - 3500) * 270.0/(11500 - 3500));
yuto17320508 12:e6fd3a3201f0 47 wait(0.01);
yuto17320508 12:e6fd3a3201f0 48 printf("servo");
yuto17320508 12:e6fd3a3201f0 49 }*/
yuto17320508 2:db2bc2ae4d20 50 pid_ro.setTolerance(10);
yuto17320508 2:db2bc2ae4d20 51 pid_ri.setTolerance(10);
yuto17320508 7:c5c60192eb02 52
yuto17320508 2:db2bc2ae4d20 53 motor_ro.setEncoder(&ec_ro);
yuto17320508 2:db2bc2ae4d20 54 motor_ro.setResolution(1000);
yuto17320508 2:db2bc2ae4d20 55 motor_ri.setEncoder(&ec_ri);
yuto17320508 2:db2bc2ae4d20 56 motor_ri.setResolution(1000);
yuto17320508 7:c5c60192eb02 57
yuto17320508 2:db2bc2ae4d20 58 leg_ro.setMotor(&motor_ro);
yuto17320508 2:db2bc2ae4d20 59 leg_ro.setPIDcontroller(&pid_ro);
yuto17320508 2:db2bc2ae4d20 60 leg_ri.setMotor(&motor_ri);
yuto17320508 2:db2bc2ae4d20 61 leg_ri.setPIDcontroller(&pid_ri);
yuto17320508 7:c5c60192eb02 62
yuto17320508 2:db2bc2ae4d20 63 robot.setLeg(&leg_ro, &leg_ri);
yuto17320508 2:db2bc2ae4d20 64 robot.setTickerTime(0.01); //モータ出力間隔 0.01
yuto17320508 7:c5c60192eb02 65
yuto17320508 6:2bc2950f32d8 66 motor_ro.setDutyLimit(0.5);
yuto17320508 6:2bc2950f32d8 67 motor_ri.setDutyLimit(0.5);
yuto17320508 7:c5c60192eb02 68
kageyuta 1:34371ffd3dc0 69 reset();
yuto17320508 10:2973cea54efd 70 for(int i=0; i<5; ++i)
yuto17320508 10:2973cea54efd 71 servo.set_degree(0,0);
yuto17320508 2:db2bc2ae4d20 72 bus_out = 1;
yuto17320508 2:db2bc2ae4d20 73 printf("start\n\r");
yuto17320508 7:c5c60192eb02 74 int mode = -1;
yuto17320508 13:678870d8f851 75 air_hand = 0;
shimizuta 9:ac95473a5d86 76
shimizuta 9:ac95473a5d86 77 while(1) {
yuto17320508 7:c5c60192eb02 78 float duty = 0.0;
yuto17320508 7:c5c60192eb02 79 can_receive(mode,duty);
yuto17320508 13:678870d8f851 80 bus_out = 0;//未収束
yuto17320508 7:c5c60192eb02 81 printf("mode:%d duty:%.3f\n\r", mode, duty);
yuto17320508 7:c5c60192eb02 82 motor_ro.setDutyLimit(duty);
yuto17320508 7:c5c60192eb02 83 motor_ri.setDutyLimit(duty);
yuto17320508 13:678870d8f851 84 //printf("target is %d\n\r",target_ro);
yuto17320508 13:678870d8f851 85 if(mode == FORWARD) forward();
yuto17320508 13:678870d8f851 86 else if(mode == STOP) stop();
yuto17320508 13:678870d8f851 87 else if(mode == BACK) back();
yuto17320508 13:678870d8f851 88 if(hand_mode == G_CLOSE) {
yuto17320508 13:678870d8f851 89 air_hand = 0;
yuto17320508 12:e6fd3a3201f0 90 }
yuto17320508 13:678870d8f851 91 else if(hand_mode == G_OPEN) {
yuto17320508 13:678870d8f851 92 air_hand = 1;
shimizuta 11:0522b336eb82 93
shimizuta 9:ac95473a5d86 94 } else if(hand_mode == GOAL) {
shimizuta 9:ac95473a5d86 95 for(int i=0; i<10; ++i)
shimizuta 9:ac95473a5d86 96 servo.set_degree(0,(7200 - 3500) * 270.0/(11500 - 3500));
yuto17320508 7:c5c60192eb02 97 }
yuto17320508 13:678870d8f851 98 bus_out = 1;//収束
eri 0:411ab20ce87d 99 }
yuto17320508 7:c5c60192eb02 100
eri 0:411ab20ce87d 101 }
shimizuta 11:0522b336eb82 102 void Run(int d_step_r)
yuto17320508 4:db1640bd0e89 103 {
shimizuta 11:0522b336eb82 104 int target_step = step_num_r + d_step_r;
shimizuta 11:0522b336eb82 105 leg_ro.setTargetPose(180+target_step*180);
shimizuta 11:0522b336eb82 106 leg_ri.setTargetPose(target_step*180);
shimizuta 11:0522b336eb82 107 led[0] = 1;
yuto17320508 7:c5c60192eb02 108 robot.run();
shimizuta 11:0522b336eb82 109 led[0] = 0;
shimizuta 11:0522b336eb82 110 step_num_r = target_step;
yuto17320508 7:c5c60192eb02 111 motor_ro_f.write(0);
yuto17320508 7:c5c60192eb02 112 motor_ro_b.write(0);
yuto17320508 7:c5c60192eb02 113 motor_ri_f.write(0);
yuto17320508 7:c5c60192eb02 114 motor_ri_b.write(0);
shimizuta 11:0522b336eb82 115
shimizuta 11:0522b336eb82 116 }
shimizuta 11:0522b336eb82 117
shimizuta 11:0522b336eb82 118 void forward()
shimizuta 11:0522b336eb82 119 {
shimizuta 11:0522b336eb82 120 Run(1);
yuto17320508 7:c5c60192eb02 121 }
yuto17320508 7:c5c60192eb02 122 void stop()
yuto17320508 7:c5c60192eb02 123 {
shimizuta 11:0522b336eb82 124 Run(0);
yuto17320508 4:db1640bd0e89 125 }
yuto17320508 7:c5c60192eb02 126
yuto17320508 7:c5c60192eb02 127 void back()
yuto17320508 4:db1640bd0e89 128 {
shimizuta 11:0522b336eb82 129 Run(-1);
yuto17320508 4:db1640bd0e89 130 }
eri 0:411ab20ce87d 131 //////////////////////////////////////////////
kageyuta 1:34371ffd3dc0 132 void setup()
kageyuta 1:34371ffd3dc0 133 {
eri 0:411ab20ce87d 134 can.frequency(1000000);
eri 0:411ab20ce87d 135 motor_ro_f.period_us(100);
eri 0:411ab20ce87d 136 motor_ro_b.period_us(100);
eri 0:411ab20ce87d 137 motor_ri_f.period_us(100);
eri 0:411ab20ce87d 138 motor_ri_b.period_us(100);
yuto17320508 13:678870d8f851 139 air_hand = 0;//ゲレゲを離す手の形にしておく
kageyuta 1:34371ffd3dc0 140 switch_ro.mode(PullUp);
kageyuta 1:34371ffd3dc0 141 switch_ri.mode(PullUp);
eri 0:411ab20ce87d 142 servo.init();
yuto17320508 7:c5c60192eb02 143 wait(0.01);
shimizuta 9:ac95473a5d86 144 for(int i=0; i<5; ++i)
yuto17320508 7:c5c60192eb02 145 servo.set_degree(0,0);
yuto17320508 7:c5c60192eb02 146 //(7200 - 3500) * 270.0/(11500 - 3500)
eri 0:411ab20ce87d 147 }
eri 0:411ab20ce87d 148
kageyuta 1:34371ffd3dc0 149 void reset()
kageyuta 1:34371ffd3dc0 150 {
kageyuta 1:34371ffd3dc0 151 while(switch_ro.read()) {
kageyuta 3:29999b02e940 152 motor_ro.output(0.3);
kageyuta 1:34371ffd3dc0 153 }
yuto17320508 7:c5c60192eb02 154 ec_ro.reset();
yuto17320508 7:c5c60192eb02 155 motor_ro.output(0.0);
yuto17320508 7:c5c60192eb02 156 printf("ro OK\n\r");
kageyuta 3:29999b02e940 157 while(switch_ri.read()) {
kageyuta 3:29999b02e940 158 motor_ri.output(0.3);
kageyuta 3:29999b02e940 159 }
yuto17320508 7:c5c60192eb02 160
yuto17320508 7:c5c60192eb02 161 ec_ri.reset();
yuto17320508 7:c5c60192eb02 162 motor_ri.output(0.0);
yuto17320508 7:c5c60192eb02 163 printf("ri OK\n\r");
eri 0:411ab20ce87d 164 }
eri 0:411ab20ce87d 165
yuto17320508 7:c5c60192eb02 166 void can_receive(int &mode, float &duty)
kageyuta 1:34371ffd3dc0 167 {
eri 0:411ab20ce87d 168 CANMessage msg;
yuto17320508 7:c5c60192eb02 169 while(1) {
yuto17320508 7:c5c60192eb02 170 //printf("roop\r\n");
eri 0:411ab20ce87d 171 if(can.read(msg)) {
eri 0:411ab20ce87d 172 if(msg.id==0) {
yuto17320508 7:c5c60192eb02 173 mode= msg.data[0]*0.1;
yuto17320508 7:c5c60192eb02 174 duty = (msg.data[0]%10)*0.1;
yuto17320508 7:c5c60192eb02 175 hand_mode= msg.data[1];
eri 0:411ab20ce87d 176 break;
eri 0:411ab20ce87d 177 }
kageyuta 1:34371ffd3dc0 178 led2=1;
eri 0:411ab20ce87d 179 } else led2=0;
eri 0:411ab20ce87d 180 }
yuto17320508 7:c5c60192eb02 181 }