yuto kawamura / Mbed 2 deprecated George_Slave_BOTHMOVE

right and left move at the same time

Dependencies:   mbed robot

main.cpp@1:34371ffd3dc0, 2019-04-27 (annotated)

Committer:
kageyuta
Date:
Sat Apr 27 05:34:43 2019 +0000
Revision:
1:34371ffd3dc0
Parent:
0:411ab20ce87d
Child:
2:db2bc2ae4d20
Sakurai_is_god

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"
eri 0:411ab20ce87d 3
eri 0:411ab20ce87d 4
eri 0:411ab20ce87d 5
kageyuta 1:34371ffd3dc0 6
eri 0:411ab20ce87d 7 ////////////関数
eri 0:411ab20ce87d 8 void setup();
eri 0:411ab20ce87d 9 void can_receive();
eri 0:411ab20ce87d 10 void pid(double,double);
eri 0:411ab20ce87d 11 void out_ro(double);
eri 0:411ab20ce87d 12 void out_ri(double);
kageyuta 1:34371ffd3dc0 13 void reset();
eri 0:411ab20ce87d 14
eri 0:411ab20ce87d 15
eri 0:411ab20ce87d 16 ////////////定数
kageyuta 1:34371ffd3dc0 17 int solution=1000;
kageyuta 1:34371ffd3dc0 18 double c_degree=0.36; //solution=500
eri 0:411ab20ce87d 19
eri 0:411ab20ce87d 20 double Kp_ro=0.01;
eri 0:411ab20ce87d 21 double Ki_ro=0;
eri 0:411ab20ce87d 22 double Kd_ro=0;
eri 0:411ab20ce87d 23 double Kp_ri=0.01;
kageyuta 1:34371ffd3dc0 24
eri 0:411ab20ce87d 25 double Ki_ri=0;
eri 0:411ab20ce87d 26 double Kd_ri=0;
eri 0:411ab20ce87d 27
eri 0:411ab20ce87d 28
eri 0:411ab20ce87d 29
eri 0:411ab20ce87d 30
eri 0:411ab20ce87d 31 ////////////変数
eri 0:411ab20ce87d 32 double target_ro=0,target_ri=0;
eri 0:411ab20ce87d 33 bool hand_mode=0;
eri 0:411ab20ce87d 34 double distance_ro_old=0,distance_ri_old=0,pile_ro=0,pile_ri=0;
eri 0:411ab20ce87d 35 double posi_ro=0,posi_ri=0;
eri 0:411ab20ce87d 36 double pre_time=0;
eri 0:411ab20ce87d 37
eri 0:411ab20ce87d 38
eri 0:411ab20ce87d 39 Timer timer;
eri 0:411ab20ce87d 40
eri 0:411ab20ce87d 41
eri 0:411ab20ce87d 42
eri 0:411ab20ce87d 43
eri 0:411ab20ce87d 44 //////////////////////////////////////////////
kageyuta 1:34371ffd3dc0 45 int main()
kageyuta 1:34371ffd3dc0 46 {
kageyuta 1:34371ffd3dc0 47
eri 0:411ab20ce87d 48 setup();
kageyuta 1:34371ffd3dc0 49 reset();
eri 0:411ab20ce87d 50 while(1) {
kageyuta 1:34371ffd3dc0 51 can_receive();
eri 0:411ab20ce87d 52 pid(target_ro,target_ri);
eri 0:411ab20ce87d 53 wait(0.01);
eri 0:411ab20ce87d 54 }
eri 0:411ab20ce87d 55 }
eri 0:411ab20ce87d 56
eri 0:411ab20ce87d 57 //////////////////////////////////////////////
kageyuta 1:34371ffd3dc0 58 void setup()
kageyuta 1:34371ffd3dc0 59 {
eri 0:411ab20ce87d 60 can.frequency(1000000);
eri 0:411ab20ce87d 61 motor_ro_f.period_us(100);
eri 0:411ab20ce87d 62 motor_ro_b.period_us(100);
eri 0:411ab20ce87d 63 motor_ri_f.period_us(100);
eri 0:411ab20ce87d 64 motor_ri_b.period_us(100);
kageyuta 1:34371ffd3dc0 65
kageyuta 1:34371ffd3dc0 66 switch_ro.mode(PullUp);
kageyuta 1:34371ffd3dc0 67 switch_ri.mode(PullUp);
kageyuta 1:34371ffd3dc0 68
eri 0:411ab20ce87d 69 servo.init();
eri 0:411ab20ce87d 70 timer.start();
eri 0:411ab20ce87d 71 }
eri 0:411ab20ce87d 72
kageyuta 1:34371ffd3dc0 73 void reset()
kageyuta 1:34371ffd3dc0 74 {
kageyuta 1:34371ffd3dc0 75 while(switch_ro.read()) {
kageyuta 1:34371ffd3dc0 76 out_ro(0.05);
kageyuta 1:34371ffd3dc0 77 }
kageyuta 1:34371ffd3dc0 78 ec_ro.reset();
kageyuta 1:34371ffd3dc0 79 out_ro(0);
kageyuta 1:34371ffd3dc0 80 while(switch_ri.read()) {
kageyuta 1:34371ffd3dc0 81 out_ri(0.05);
kageyuta 1:34371ffd3dc0 82 }
kageyuta 1:34371ffd3dc0 83
kageyuta 1:34371ffd3dc0 84 ec_ri.reset();
kageyuta 1:34371ffd3dc0 85 out_ri(0);
kageyuta 1:34371ffd3dc0 86 }
eri 0:411ab20ce87d 87
kageyuta 1:34371ffd3dc0 88 void pid(double target_ro_,double target_ri_)
kageyuta 1:34371ffd3dc0 89 {
eri 0:411ab20ce87d 90 posi_ro=(ec_ro.getCount()%solution)*c_degree;
kageyuta 1:34371ffd3dc0 91 if(posi_ro<0)posi_ro+=360;
eri 0:411ab20ce87d 92 posi_ri=(ec_ri.getCount()%solution)*c_degree;
kageyuta 1:34371ffd3dc0 93 if(posi_ri<0)posi_ri+=360;
kageyuta 1:34371ffd3dc0 94
eri 0:411ab20ce87d 95 double now=timer.read();
eri 0:411ab20ce87d 96 double d_time=now-pre_time;
kageyuta 1:34371ffd3dc0 97
eri 0:411ab20ce87d 98 double deviation_ro=fabs(target_ro_)-posi_ro;
kageyuta 1:34371ffd3dc0 99 if(fabs(deviation_ro)<90) { //そのまま
kageyuta 1:34371ffd3dc0 100 } else if(deviation_ro>270) {
kageyuta 1:34371ffd3dc0 101 deviation_ro-=360;
kageyuta 1:34371ffd3dc0 102 } else if(deviation_ro<-270) {
kageyuta 1:34371ffd3dc0 103 deviation_ro+=360;
kageyuta 1:34371ffd3dc0 104 } else if(target_ro_>0) {
kageyuta 1:34371ffd3dc0 105 if(deviation_ro<0)deviation_ro+=360;
kageyuta 1:34371ffd3dc0 106 } else {
kageyuta 1:34371ffd3dc0 107 if(deviation_ro>0)deviation_ro-=360;
eri 0:411ab20ce87d 108 }
kageyuta 1:34371ffd3dc0 109
eri 0:411ab20ce87d 110 double deviation_ri=fabs(target_ri_)-posi_ri;
kageyuta 1:34371ffd3dc0 111 if(fabs(deviation_ri)<90) { //そのまま
kageyuta 1:34371ffd3dc0 112 } else if(deviation_ri>270) {
kageyuta 1:34371ffd3dc0 113 deviation_ri-=360;
kageyuta 1:34371ffd3dc0 114 } else if(deviation_ri<-270) {
kageyuta 1:34371ffd3dc0 115 deviation_ri+=360;
kageyuta 1:34371ffd3dc0 116 } else if(target_ri_>0) {
kageyuta 1:34371ffd3dc0 117 if(deviation_ri<0)deviation_ri+=360;
kageyuta 1:34371ffd3dc0 118 } else {
kageyuta 1:34371ffd3dc0 119 if(deviation_ri>0)deviation_ri-=360;
eri 0:411ab20ce87d 120 }
kageyuta 1:34371ffd3dc0 121
eri 0:411ab20ce87d 122 pile_ro+=deviation_ro;
eri 0:411ab20ce87d 123 pile_ri+=deviation_ri;
kageyuta 1:34371ffd3dc0 124
eri 0:411ab20ce87d 125 out_ro(deviation_ro * Kp_ro + (posi_ro - distance_ro_old) / d_time * Kd_ro + pile_ro * Ki_ro * d_time);
eri 0:411ab20ce87d 126 out_ri(deviation_ri * Kp_ri + (posi_ri - distance_ri_old) / d_time * Kd_ri + pile_ri * Ki_ri * d_time);
kageyuta 1:34371ffd3dc0 127
eri 0:411ab20ce87d 128 distance_ro_old=deviation_ro;
eri 0:411ab20ce87d 129 distance_ri_old=deviation_ri;
eri 0:411ab20ce87d 130 pre_time=now;
eri 0:411ab20ce87d 131 }
eri 0:411ab20ce87d 132
eri 0:411ab20ce87d 133
kageyuta 1:34371ffd3dc0 134 void out_ro(double duty)
kageyuta 1:34371ffd3dc0 135 {
kageyuta 1:34371ffd3dc0 136 double dutylimit=0.1;
eri 0:411ab20ce87d 137
eri 0:411ab20ce87d 138 if(duty > 0) { //入力duty比が正の場合
eri 0:411ab20ce87d 139 //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max;
eri 0:411ab20ce87d 140 if( fabs( duty ) < dutylimit ) { //制限値内
eri 0:411ab20ce87d 141 motor_ro_f = fabs(duty);
eri 0:411ab20ce87d 142 motor_ro_b = 0;
eri 0:411ab20ce87d 143 } else { //制限値超
eri 0:411ab20ce87d 144 motor_ro_f = dutylimit;
eri 0:411ab20ce87d 145 motor_ro_b = 0;
eri 0:411ab20ce87d 146 }
eri 0:411ab20ce87d 147 } else {//入力duty比が負の場合
eri 0:411ab20ce87d 148 //if(pre_out_r-duty >accel_max && pre_out_l*duty>0)duty=pre_out_r-accel_max;
eri 0:411ab20ce87d 149 if( fabs(duty) < dutylimit) { //制限値内
eri 0:411ab20ce87d 150 motor_ro_f = 0;
eri 0:411ab20ce87d 151 motor_ro_b = fabs(duty);
eri 0:411ab20ce87d 152 } else { //制限値超
eri 0:411ab20ce87d 153 motor_ro_f = 0;
eri 0:411ab20ce87d 154 motor_ro_b = dutylimit;
eri 0:411ab20ce87d 155 }
eri 0:411ab20ce87d 156 }
eri 0:411ab20ce87d 157 //pre_out_r=duty;
eri 0:411ab20ce87d 158 }
eri 0:411ab20ce87d 159
eri 0:411ab20ce87d 160
kageyuta 1:34371ffd3dc0 161 void out_ri(double duty)
kageyuta 1:34371ffd3dc0 162 {
kageyuta 1:34371ffd3dc0 163 double dutylimit=0.1;
eri 0:411ab20ce87d 164
eri 0:411ab20ce87d 165 if(duty > 0) { //入力duty比が正の場合
eri 0:411ab20ce87d 166 //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max;
eri 0:411ab20ce87d 167 if( fabs( duty ) < dutylimit ) { //制限値内
eri 0:411ab20ce87d 168 motor_ri_f = fabs(duty);
eri 0:411ab20ce87d 169 motor_ri_b = 0;
eri 0:411ab20ce87d 170 } else { //制限値超
eri 0:411ab20ce87d 171 motor_ri_f = dutylimit;
eri 0:411ab20ce87d 172 motor_ri_b = 0;
eri 0:411ab20ce87d 173 }
eri 0:411ab20ce87d 174 } else {//入力duty比が負の場合
eri 0:411ab20ce87d 175 //if(pre_out_r-duty >accel_max && pre_out_l*duty>0)duty=pre_out_r-accel_max;
eri 0:411ab20ce87d 176 if( fabs(duty) < dutylimit) { //制限値内
eri 0:411ab20ce87d 177 motor_ri_f = 0;
eri 0:411ab20ce87d 178 motor_ri_b = fabs(duty);
eri 0:411ab20ce87d 179 } else { //制限値超
eri 0:411ab20ce87d 180 motor_ri_f = 0;
eri 0:411ab20ce87d 181 motor_ri_b = dutylimit;
eri 0:411ab20ce87d 182 }
eri 0:411ab20ce87d 183 }
eri 0:411ab20ce87d 184 //pre_out_r=duty;
eri 0:411ab20ce87d 185 }
eri 0:411ab20ce87d 186
eri 0:411ab20ce87d 187 //////////////////////////////////////////can
kageyuta 1:34371ffd3dc0 188 void can_receive()
kageyuta 1:34371ffd3dc0 189 {
eri 0:411ab20ce87d 190 CANMessage msg;
eri 0:411ab20ce87d 191 for(int i=0; i<5; i++) {
eri 0:411ab20ce87d 192 if(can.read(msg)) {
eri 0:411ab20ce87d 193 if(msg.id==0) {
kageyuta 1:34371ffd3dc0 194 target_ro= msg.data[0] + ((msg.data[2]&0b1111)<<8) - 360;
kageyuta 1:34371ffd3dc0 195 target_ri= msg.data[1] + ((msg.data[2]&0b11110000)<<4) - 360;
eri 0:411ab20ce87d 196 hand_mode= msg.data[3];
kageyuta 1:34371ffd3dc0 197
eri 0:411ab20ce87d 198 break;
eri 0:411ab20ce87d 199 }
kageyuta 1:34371ffd3dc0 200 led2=1;
eri 0:411ab20ce87d 201 } else led2=0;
eri 0:411ab20ce87d 202 }
eri 0:411ab20ce87d 203 }