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