yuto kawamura / Mbed 2 deprecated MR2_2n_1_saku

sls

Dependencies:   mbed

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?

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 ////////////定数
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 }