yuto kawamura / Mbed 2 deprecated MR2_2n_1_saku

sls

Dependencies:   mbed

main.cpp@4:96f38805f055, 2019-04-29 (annotated)

Committer:
yuto17320508
Date:
Mon Apr 29 07:02:33 2019 +0000
Revision:
4:96f38805f055
Parent:
3:7a608fbd3bcd 
ss

Who changed what in which revision?

UserRevisionLine numberNew 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()) {
yuto17320508 4:96f38805f055 117 out_lo(0.12);
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()) {
yuto17320508 4:96f38805f055 122 out_li(0.10);
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
yuto17320508 4:96f38805f055 132 double dutylimit=0.2;
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
yuto17320508 4:96f38805f055 160 double dutylimit=0.2;
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 }