yuto kawamura
/
MR2_2n_1_saku
sls
main.cpp@3:7a608fbd3bcd, 2019-04-27 (annotated)
- Committer:
- yuto17320508
- Date:
- Sat Apr 27 08:13:01 2019 +0000
- Revision:
- 3:7a608fbd3bcd
- Parent:
- 2:55c616d2e0fe
- Child:
- 4:96f38805f055
fight for NHK
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
eri | 0:c1476d342c13 | 1 | #include "mbed.h" |
eri | 0:c1476d342c13 | 2 | #include "pin.h" |
eri | 0:c1476d342c13 | 3 | #include "microinfinity.h" |
eri | 0:c1476d342c13 | 4 | |
eri | 0:c1476d342c13 | 5 | |
eri | 0:c1476d342c13 | 6 | |
eri | 0:c1476d342c13 | 7 | ////////////関数 |
eri | 0:c1476d342c13 | 8 | void setup(); |
eri | 0:c1476d342c13 | 9 | void can_send(); |
kageyuta | 1:86c4c38abe40 | 10 | void pid(double,double); |
kageyuta | 1:86c4c38abe40 | 11 | void out_lo(double); |
kageyuta | 1:86c4c38abe40 | 12 | void out_li(double); |
kageyuta | 2:55c616d2e0fe | 13 | void reset(); |
eri | 0:c1476d342c13 | 14 | |
eri | 0:c1476d342c13 | 15 | ////////////定数 |
yuto17320508 | 3:7a608fbd3bcd | 16 | int resolution_lo=1000; |
yuto17320508 | 3:7a608fbd3bcd | 17 | int resolution_li=600; |
yuto17320508 | 3:7a608fbd3bcd | 18 | double c_degree_lo=0.36; //solution=500 |
yuto17320508 | 3:7a608fbd3bcd | 19 | double c_degree_li=360.0/600.0; |
eri | 0:c1476d342c13 | 20 | |
kageyuta | 1:86c4c38abe40 | 21 | double Kp_lo=0.01; |
kageyuta | 1:86c4c38abe40 | 22 | double Ki_lo=0; |
kageyuta | 1:86c4c38abe40 | 23 | double Kd_lo=0; |
kageyuta | 1:86c4c38abe40 | 24 | double Kp_li=0.01; |
eri | 0:c1476d342c13 | 25 | |
kageyuta | 1:86c4c38abe40 | 26 | double Ki_li=0; |
kageyuta | 1:86c4c38abe40 | 27 | double Kd_li=0; |
eri | 0:c1476d342c13 | 28 | |
eri | 0:c1476d342c13 | 29 | |
eri | 0:c1476d342c13 | 30 | |
eri | 0:c1476d342c13 | 31 | ////////////変数 |
eri | 0:c1476d342c13 | 32 | double target_ro=0,target_ri=0; |
eri | 0:c1476d342c13 | 33 | double target_lo=0,target_li=0; |
eri | 0:c1476d342c13 | 34 | bool hand_mode=0; |
kageyuta | 1:86c4c38abe40 | 35 | double distance_lo_old=0,distance_li_old=0,pile_lo=0,pile_li=0; |
kageyuta | 1:86c4c38abe40 | 36 | double posi_lo=0,posi_li=0; |
kageyuta | 1:86c4c38abe40 | 37 | double pre_time=0; |
kageyuta | 1:86c4c38abe40 | 38 | |
kageyuta | 1:86c4c38abe40 | 39 | Timer timer; |
kageyuta | 1:86c4c38abe40 | 40 | |
eri | 0:c1476d342c13 | 41 | |
eri | 0:c1476d342c13 | 42 | |
eri | 0:c1476d342c13 | 43 | |
eri | 0:c1476d342c13 | 44 | ///////////////////////////////////////////// |
kageyuta | 1:86c4c38abe40 | 45 | int main() |
kageyuta | 1:86c4c38abe40 | 46 | { |
kageyuta | 1:86c4c38abe40 | 47 | |
eri | 0:c1476d342c13 | 48 | setup(); |
kageyuta | 2:55c616d2e0fe | 49 | reset(); |
yuto17320508 | 3:7a608fbd3bcd | 50 | while(1){ |
yuto17320508 | 3:7a608fbd3bcd | 51 | target_lo = 180; |
yuto17320508 | 3:7a608fbd3bcd | 52 | target_li = 360; |
yuto17320508 | 3:7a608fbd3bcd | 53 | target_ri = 360; |
yuto17320508 | 3:7a608fbd3bcd | 54 | target_ro = 180; |
yuto17320508 | 3:7a608fbd3bcd | 55 | for(int i=0;i<100;++i){ |
kageyuta | 1:86c4c38abe40 | 56 | can_send(); |
kageyuta | 1:86c4c38abe40 | 57 | pid(target_lo,target_li); |
yuto17320508 | 3:7a608fbd3bcd | 58 | //printf("%f,%f\r\n",posi_li,posi_lo); |
yuto17320508 | 3:7a608fbd3bcd | 59 | wait(0.01); |
yuto17320508 | 3:7a608fbd3bcd | 60 | } |
yuto17320508 | 3:7a608fbd3bcd | 61 | target_lo = 360; |
yuto17320508 | 3:7a608fbd3bcd | 62 | target_li = 180; |
yuto17320508 | 3:7a608fbd3bcd | 63 | target_ri = 180; |
yuto17320508 | 3:7a608fbd3bcd | 64 | target_ro = 360; |
yuto17320508 | 3:7a608fbd3bcd | 65 | for(int i=0;i<100;++i){ |
yuto17320508 | 3:7a608fbd3bcd | 66 | can_send(); |
yuto17320508 | 3:7a608fbd3bcd | 67 | pid(target_lo,target_li); |
yuto17320508 | 3:7a608fbd3bcd | 68 | //printf("%f,%f\r\n",posi_li,posi_lo); |
yuto17320508 | 3:7a608fbd3bcd | 69 | wait(0.01); |
yuto17320508 | 3:7a608fbd3bcd | 70 | } |
eri | 0:c1476d342c13 | 71 | } |
eri | 0:c1476d342c13 | 72 | } |
eri | 0:c1476d342c13 | 73 | |
eri | 0:c1476d342c13 | 74 | |
kageyuta | 1:86c4c38abe40 | 75 | void setup() |
kageyuta | 1:86c4c38abe40 | 76 | { |
eri | 0:c1476d342c13 | 77 | can1.frequency(1000000); |
eri | 0:c1476d342c13 | 78 | motor_lo_f.period_us(100); |
eri | 0:c1476d342c13 | 79 | motor_lo_b.period_us(100); |
eri | 0:c1476d342c13 | 80 | motor_li_f.period_us(100); |
eri | 0:c1476d342c13 | 81 | motor_li_b.period_us(100); |
kageyuta | 1:86c4c38abe40 | 82 | |
eri | 0:c1476d342c13 | 83 | hand.mode(PullUp); |
kageyuta | 2:55c616d2e0fe | 84 | switch_lo.mode(PullUp); |
kageyuta | 2:55c616d2e0fe | 85 | switch_li.mode(PullUp); |
eri | 0:c1476d342c13 | 86 | switch4.mode(PullUp); |
kageyuta | 1:86c4c38abe40 | 87 | |
eri | 0:c1476d342c13 | 88 | |
eri | 0:c1476d342c13 | 89 | device.baud(115200); |
eri | 0:c1476d342c13 | 90 | device.format(8,Serial::None,1); |
eri | 0:c1476d342c13 | 91 | device.attach(dev_rx, Serial::RxIrq); |
eri | 0:c1476d342c13 | 92 | wait(0.05); |
eri | 0:c1476d342c13 | 93 | theta0=degree0; |
eri | 0:c1476d342c13 | 94 | check_gyro(); |
kageyuta | 1:86c4c38abe40 | 95 | timer.start(); |
eri | 0:c1476d342c13 | 96 | } |
eri | 0:c1476d342c13 | 97 | |
eri | 0:c1476d342c13 | 98 | |
eri | 0:c1476d342c13 | 99 | //////////////////////////////////////can |
eri | 0:c1476d342c13 | 100 | void can_send() |
eri | 0:c1476d342c13 | 101 | { |
kageyuta | 1:86c4c38abe40 | 102 | char data[4]= {0}; |
eri | 0:c1476d342c13 | 103 | int target_ro_send=target_ro+360; |
eri | 0:c1476d342c13 | 104 | int target_ri_send=target_ri+360; |
eri | 0:c1476d342c13 | 105 | data[0]=target_ro_send & 0b11111111; |
eri | 0:c1476d342c13 | 106 | data[1]=target_ri_send & 0b11111111; |
eri | 0:c1476d342c13 | 107 | data[2]=(target_ro_send>>8) | ((target_ri_send>>4) & 0b11110000); |
eri | 0:c1476d342c13 | 108 | data[3]=hand_mode; |
kageyuta | 1:86c4c38abe40 | 109 | |
eri | 0:c1476d342c13 | 110 | if(can1.write(CANMessage(0,data,4)))led4=1; |
eri | 0:c1476d342c13 | 111 | else led4=0; |
kageyuta | 1:86c4c38abe40 | 112 | } |
kageyuta | 1:86c4c38abe40 | 113 | |
kageyuta | 2:55c616d2e0fe | 114 | void reset() |
kageyuta | 2:55c616d2e0fe | 115 | { |
kageyuta | 2:55c616d2e0fe | 116 | while(switch_lo.read()) { |
kageyuta | 2:55c616d2e0fe | 117 | out_lo(0.05); |
kageyuta | 2:55c616d2e0fe | 118 | } |
kageyuta | 2:55c616d2e0fe | 119 | ec_lo.reset(); |
kageyuta | 2:55c616d2e0fe | 120 | out_lo(0); |
kageyuta | 2:55c616d2e0fe | 121 | while(switch_li.read()) { |
kageyuta | 2:55c616d2e0fe | 122 | out_li(0.05); |
kageyuta | 2:55c616d2e0fe | 123 | } |
kageyuta | 2:55c616d2e0fe | 124 | |
kageyuta | 2:55c616d2e0fe | 125 | ec_li.reset(); |
kageyuta | 2:55c616d2e0fe | 126 | out_li(0); |
kageyuta | 2:55c616d2e0fe | 127 | } |
kageyuta | 1:86c4c38abe40 | 128 | |
kageyuta | 1:86c4c38abe40 | 129 | void out_lo(double duty) |
kageyuta | 1:86c4c38abe40 | 130 | { |
kageyuta | 1:86c4c38abe40 | 131 | |
kageyuta | 1:86c4c38abe40 | 132 | double dutylimit=0.1; |
kageyuta | 1:86c4c38abe40 | 133 | |
kageyuta | 1:86c4c38abe40 | 134 | if(duty > 0) { //入力duty比が正の場合 |
kageyuta | 1:86c4c38abe40 | 135 | //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max; |
kageyuta | 1:86c4c38abe40 | 136 | if( fabs( duty ) < dutylimit ) { //制限値内 |
kageyuta | 1:86c4c38abe40 | 137 | motor_lo_f = fabs(duty); |
kageyuta | 1:86c4c38abe40 | 138 | motor_lo_b = 0; |
kageyuta | 1:86c4c38abe40 | 139 | } else { //制限値超 |
kageyuta | 1:86c4c38abe40 | 140 | motor_lo_f = dutylimit; |
kageyuta | 1:86c4c38abe40 | 141 | motor_lo_b = 0; |
kageyuta | 1:86c4c38abe40 | 142 | } |
kageyuta | 1:86c4c38abe40 | 143 | } else {//入力duty比が負の場合 |
kageyuta | 1:86c4c38abe40 | 144 | //if(pre_out_r-duty >accel_max && pre_out_l*duty>0)duty=pre_out_r-accel_max; |
kageyuta | 1:86c4c38abe40 | 145 | if( fabs(duty) < dutylimit) { //制限値内 |
kageyuta | 1:86c4c38abe40 | 146 | motor_lo_f = 0; |
kageyuta | 1:86c4c38abe40 | 147 | motor_lo_b = fabs(duty); |
kageyuta | 1:86c4c38abe40 | 148 | } else { //制限値超 |
kageyuta | 1:86c4c38abe40 | 149 | motor_lo_f = 0; |
kageyuta | 1:86c4c38abe40 | 150 | motor_lo_b = dutylimit; |
kageyuta | 1:86c4c38abe40 | 151 | } |
kageyuta | 1:86c4c38abe40 | 152 | } |
kageyuta | 1:86c4c38abe40 | 153 | //pre_out_r=duty; |
kageyuta | 1:86c4c38abe40 | 154 | } |
kageyuta | 1:86c4c38abe40 | 155 | |
kageyuta | 1:86c4c38abe40 | 156 | |
kageyuta | 1:86c4c38abe40 | 157 | void out_li(double duty) |
kageyuta | 1:86c4c38abe40 | 158 | { |
kageyuta | 1:86c4c38abe40 | 159 | |
kageyuta | 1:86c4c38abe40 | 160 | double dutylimit=0.1; |
kageyuta | 1:86c4c38abe40 | 161 | |
kageyuta | 1:86c4c38abe40 | 162 | if(duty > 0) { //入力duty比が正の場合 |
kageyuta | 1:86c4c38abe40 | 163 | //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max; |
kageyuta | 1:86c4c38abe40 | 164 | if( fabs( duty ) < dutylimit ) { //制限値内 |
kageyuta | 1:86c4c38abe40 | 165 | motor_li_f = fabs(duty); |
kageyuta | 1:86c4c38abe40 | 166 | motor_li_b = 0; |
kageyuta | 1:86c4c38abe40 | 167 | } else { //制限値超 |
kageyuta | 1:86c4c38abe40 | 168 | motor_li_f = dutylimit; |
kageyuta | 1:86c4c38abe40 | 169 | motor_li_b = 0; |
kageyuta | 1:86c4c38abe40 | 170 | } |
kageyuta | 1:86c4c38abe40 | 171 | } else {//入力duty比が負の場合 |
kageyuta | 1:86c4c38abe40 | 172 | //if(pre_out_r-duty >accel_max && pre_out_l*duty>0)duty=pre_out_r-accel_max; |
kageyuta | 1:86c4c38abe40 | 173 | if( fabs(duty) < dutylimit) { //制限値内 |
kageyuta | 1:86c4c38abe40 | 174 | motor_li_f = 0; |
kageyuta | 1:86c4c38abe40 | 175 | motor_li_b = fabs(duty); |
kageyuta | 1:86c4c38abe40 | 176 | } else { //制限値超 |
kageyuta | 1:86c4c38abe40 | 177 | motor_li_f = 0; |
kageyuta | 1:86c4c38abe40 | 178 | motor_li_b = dutylimit; |
kageyuta | 1:86c4c38abe40 | 179 | } |
kageyuta | 1:86c4c38abe40 | 180 | } |
kageyuta | 1:86c4c38abe40 | 181 | //pre_out_r=duty; |
kageyuta | 1:86c4c38abe40 | 182 | } |
kageyuta | 1:86c4c38abe40 | 183 | |
kageyuta | 1:86c4c38abe40 | 184 | void pid(double target_lo_,double target_li_) |
kageyuta | 1:86c4c38abe40 | 185 | { |
yuto17320508 | 3:7a608fbd3bcd | 186 | posi_lo=(ec_lo.getCount()%resolution_lo)*c_degree_lo; |
kageyuta | 1:86c4c38abe40 | 187 | if(posi_lo<0)posi_lo+=360; |
yuto17320508 | 3:7a608fbd3bcd | 188 | posi_li=(ec_li.getCount()%resolution_li)*c_degree_li; |
kageyuta | 1:86c4c38abe40 | 189 | if(posi_li<0)posi_li+=360; |
kageyuta | 1:86c4c38abe40 | 190 | |
kageyuta | 1:86c4c38abe40 | 191 | double now=timer.read(); |
kageyuta | 1:86c4c38abe40 | 192 | double d_time=now-pre_time; |
kageyuta | 1:86c4c38abe40 | 193 | |
kageyuta | 1:86c4c38abe40 | 194 | double deviation_lo=fabs(target_lo_)-posi_lo; |
kageyuta | 1:86c4c38abe40 | 195 | if(fabs(deviation_lo)<90) { //そのまま |
kageyuta | 1:86c4c38abe40 | 196 | } else if(deviation_lo>270) { |
kageyuta | 1:86c4c38abe40 | 197 | deviation_lo-=360; |
kageyuta | 1:86c4c38abe40 | 198 | } else if(deviation_lo<-270) { |
kageyuta | 1:86c4c38abe40 | 199 | deviation_lo+=360; |
kageyuta | 1:86c4c38abe40 | 200 | } else if(target_lo_>0) { |
kageyuta | 1:86c4c38abe40 | 201 | if(deviation_lo<0)deviation_lo+=360; |
kageyuta | 1:86c4c38abe40 | 202 | } else { |
kageyuta | 1:86c4c38abe40 | 203 | if(deviation_lo>0)deviation_lo-=360; |
kageyuta | 1:86c4c38abe40 | 204 | } |
kageyuta | 1:86c4c38abe40 | 205 | |
kageyuta | 1:86c4c38abe40 | 206 | double deviation_li=fabs(target_li_)-posi_li; |
kageyuta | 1:86c4c38abe40 | 207 | if(fabs(deviation_li)<90) { //そのまま |
kageyuta | 1:86c4c38abe40 | 208 | } else if(deviation_li>270) { |
kageyuta | 1:86c4c38abe40 | 209 | deviation_li-=360; |
kageyuta | 1:86c4c38abe40 | 210 | } else if(deviation_li<-270) { |
kageyuta | 1:86c4c38abe40 | 211 | deviation_li+=360; |
kageyuta | 1:86c4c38abe40 | 212 | } else if(target_li_>0) { |
kageyuta | 1:86c4c38abe40 | 213 | if(deviation_li<0)deviation_li+=360; |
kageyuta | 1:86c4c38abe40 | 214 | } else { |
kageyuta | 1:86c4c38abe40 | 215 | if(deviation_li>0)deviation_li-=360; |
kageyuta | 1:86c4c38abe40 | 216 | } |
kageyuta | 1:86c4c38abe40 | 217 | |
kageyuta | 1:86c4c38abe40 | 218 | pile_lo+=deviation_lo; |
kageyuta | 1:86c4c38abe40 | 219 | pile_li+=deviation_li; |
kageyuta | 1:86c4c38abe40 | 220 | |
kageyuta | 1:86c4c38abe40 | 221 | out_lo(deviation_lo * Kp_lo + (posi_lo - distance_lo_old) / d_time * Kd_lo + pile_lo * Ki_lo * d_time); |
kageyuta | 1:86c4c38abe40 | 222 | out_li(deviation_li * Kp_li + (posi_li - distance_li_old) / d_time * Kd_li + pile_li * Ki_li * d_time); |
kageyuta | 1:86c4c38abe40 | 223 | |
kageyuta | 1:86c4c38abe40 | 224 | distance_lo_old=deviation_lo; |
kageyuta | 1:86c4c38abe40 | 225 | distance_li_old=deviation_li; |
kageyuta | 1:86c4c38abe40 | 226 | pre_time=now; |
eri | 0:c1476d342c13 | 227 | } |