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