yuto kawamura / Mbed 2 deprecated MR2_2n_1_saku

sls

Dependencies:   mbed

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?

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);
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 }