NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2019_main_v6

NHK2019_Team_B_Automatic_machine_maingawa

Dependencies:   SerialMultiByte QEI omni_wheel PID R1370MeasuringWheel IRsensor ikarashiMDC_2byte_ver

main.cpp@1:697097e548f9, 2019-10-02 (annotated)

Committer:
skouki
Date:
Wed Oct 02 10:00:19 2019 +0000
Revision:
1:697097e548f9
Parent:
0:f68b460de813 
v6;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skouki 0:f68b460de813 1 #include"mbed.h"
skouki 0:f68b460de813 2 #include"SerialMultiByte.h"
skouki 0:f68b460de813 3 #include"pin_config.h"
skouki 1:697097e548f9 4 #include"IRsensor.h"
skouki 1:697097e548f9 5 #include"QEI.h"
skouki 1:697097e548f9 6 #include"PID.h"
skouki 1:697097e548f9 7 #include"ikarashiMDC.h"
skouki 1:697097e548f9 8
skouki 1:697097e548f9 9 #define POINT 22380
skouki 0:f68b460de813 10
skouki 0:f68b460de813 11 SerialMultiByte sita_s(S_SERIALTX,S_SERIALRX);
skouki 0:f68b460de813 12 SerialMultiByte ue_s(U_SERIALTX,U_SERIALRX);
skouki 1:697097e548f9 13 DigitalIn start(STARTPIN);
skouki 0:f68b460de813 14 Serial pc(USBTX,USBRX,115200);
skouki 0:f68b460de813 15
skouki 0:f68b460de813 16 DigitalOut debug_led_0(LED0);
skouki 0:f68b460de813 17 DigitalOut debug_led_1(LED1);
skouki 0:f68b460de813 18 DigitalOut debug_led_2(LED2);
skouki 1:697097e548f9 19 DigitalIn mode_r_b(MODE_R_B);
skouki 1:697097e548f9 20 PID pid(5.0,3.0,0.000001,0.001);
skouki 0:f68b460de813 21
skouki 1:697097e548f9 22 Serial serial(MDCTX,MDCRX,115200);
skouki 1:697097e548f9 23 ikarashiMDC motor[]={
skouki 1:697097e548f9 24 ikarashiMDC(1,0,SM,&serial),
skouki 1:697097e548f9 25 ikarashiMDC(1,1,SM,&serial),
skouki 1:697097e548f9 26 ikarashiMDC(1,2,SM,&serial),
skouki 1:697097e548f9 27 ikarashiMDC(1,3,SM,&serial)
skouki 1:697097e548f9 28 };
skouki 1:697097e548f9 29
skouki 1:697097e548f9 30 QEI wheel_2(QEI_1_1,QEI_1_2, NC, 500,QEI::X4_ENCODING);
skouki 1:697097e548f9 31 QEI wheel_1(QEI_3_1,QEI_3_2, NC, 500,QEI::X4_ENCODING);
skouki 1:697097e548f9 32 /*
skouki 1:697097e548f9 33 IRsensor IR0(IR_0);
skouki 1:697097e548f9 34 IRsensor IR1(IR_1);
skouki 1:697097e548f9 35 IRsensor IR2(IR_2);
skouki 1:697097e548f9 36 */
skouki 0:f68b460de813 37 int mode=0;
skouki 0:f68b460de813 38 int u_mode=0,s_mode=0,m_mode=0;
skouki 0:f68b460de813 39 unsigned char itidata[4];
skouki 0:f68b460de813 40 int X_,Y_;
skouki 0:f68b460de813 41 int data_a;
skouki 1:697097e548f9 42 double power[4];
skouki 1:697097e548f9 43 bool end_flag = true;
skouki 1:697097e548f9 44 bool down_flag = false;
skouki 1:697097e548f9 45 bool debugmode = false;
skouki 1:697097e548f9 46 int height_point = POINT;
skouki 1:697097e548f9 47 double plus_power;
skouki 1:697097e548f9 48 int field;
skouki 0:f68b460de813 49
skouki 0:f68b460de813 50 void to_sita(){
skouki 1:697097e548f9 51 unsigned char data[6];
skouki 0:f68b460de813 52 unsigned char getdata[1];
skouki 0:f68b460de813 53 data[0] = mode;
skouki 0:f68b460de813 54 for(int i=0;i<4;i++){
skouki 0:f68b460de813 55 data[i+1] = itidata[i];
skouki 0:f68b460de813 56 }
skouki 1:697097e548f9 57 data[5] = field;
skouki 1:697097e548f9 58 sita_s.sendData(data,6);
skouki 0:f68b460de813 59 sita_s.getData(getdata);
skouki 0:f68b460de813 60 s_mode = getdata[0];
skouki 0:f68b460de813 61 }
skouki 1:697097e548f9 62 /*
skouki 1:697097e548f9 63 void ir_checker(){
skouki 1:697097e548f9 64 float ir_0 = IR0.get_Averagingdistance();
skouki 1:697097e548f9 65 float ir_1 = IR1.get_Averagingdistance();
skouki 1:697097e548f9 66 float ir_2 = IR2.get_Averagingdistance();
skouki 1:697097e548f9 67 pc.printf("%.2f\\%.2f\\%.2f\\",ir_0,ir_1,ir_2);
skouki 0:f68b460de813 68
skouki 1:697097e548f9 69 data_a = 2;
skouki 1:697097e548f9 70 if(ir_0 <= 40.0) {
skouki 1:697097e548f9 71 data_a = 1;
skouki 1:697097e548f9 72 return;
skouki 1:697097e548f9 73 }
skouki 1:697097e548f9 74 if(ir_2 <= 40.0){
skouki 1:697097e548f9 75 data_a = 3;
skouki 1:697097e548f9 76 return;
skouki 1:697097e548f9 77 }
skouki 1:697097e548f9 78 data_a = 0;
skouki 1:697097e548f9 79 }
skouki 1:697097e548f9 80 */
skouki 0:f68b460de813 81 void to_ue(){
skouki 1:697097e548f9 82 unsigned char data[2];
skouki 0:f68b460de813 83 unsigned char getdata[5];
skouki 0:f68b460de813 84 data[0] = mode;
skouki 1:697097e548f9 85 data[1] = field;
skouki 1:697097e548f9 86 ue_s.sendData(data,2);
skouki 0:f68b460de813 87 ue_s.getData(getdata);
skouki 0:f68b460de813 88 u_mode=getdata[0];
skouki 0:f68b460de813 89 for(int i=0;i<4;i++){
skouki 0:f68b460de813 90 itidata[i] = getdata[i+1];
skouki 0:f68b460de813 91 }
skouki 0:f68b460de813 92 }
skouki 0:f68b460de813 93
skouki 1:697097e548f9 94 void syoukou()
skouki 1:697097e548f9 95 {
skouki 0:f68b460de813 96
skouki 1:697097e548f9 97 if(debugmode)return;
skouki 1:697097e548f9 98 if(end_flag&&!down_flag){
skouki 1:697097e548f9 99 if(height_point >= wheel_1.getPulses())power[0] = 0.8;
skouki 1:697097e548f9 100 else power[0] = 0.0;
skouki 1:697097e548f9 101 if(height_point >= wheel_2.getPulses()){
skouki 1:697097e548f9 102 pid.setProcessValue(wheel_1.getPulses() - wheel_2.getPulses());
skouki 1:697097e548f9 103 plus_power = pid.compute();
skouki 1:697097e548f9 104 power[1] = 0.8 - plus_power;
skouki 1:697097e548f9 105 }
skouki 1:697097e548f9 106 else power[1] = 0.0;
skouki 1:697097e548f9 107 if((height_point <= wheel_1.getPulses())&&(height_point <= wheel_2.getPulses()))down_flag = true;
skouki 1:697097e548f9 108 }
skouki 1:697097e548f9 109 if(!end_flag){
skouki 1:697097e548f9 110 power[0] += 0.0;
skouki 1:697097e548f9 111 power[1] += 0.0;
skouki 1:697097e548f9 112 }
skouki 1:697097e548f9 113 if(down_flag){
skouki 1:697097e548f9 114 if((height_point-3000) < wheel_1.getPulses())power[0] -= 0.6;
skouki 1:697097e548f9 115 else power[0] = 0.0;
skouki 1:697097e548f9 116 if((height_point-3000) < wheel_2.getPulses())power[1] -= 0.6;
skouki 1:697097e548f9 117 else power[1] = 0.0;
skouki 1:697097e548f9 118 if(power[0]+power[1] == 0.0){
skouki 1:697097e548f9 119 down_flag = false;
skouki 1:697097e548f9 120 end_flag = false;
skouki 1:697097e548f9 121 }
skouki 1:697097e548f9 122 }
skouki 1:697097e548f9 123 }
skouki 1:697097e548f9 124 void iti_checker()
skouki 1:697097e548f9 125 {
skouki 1:697097e548f9 126 int X_;
skouki 1:697097e548f9 127 if(itidata[1]>=(1<<7)){
skouki 1:697097e548f9 128 itidata[1]-=(1<<7);
skouki 1:697097e548f9 129 X_=-1*(itidata[0]+(itidata[1]<<8));
skouki 1:697097e548f9 130 }
skouki 1:697097e548f9 131 else X_= itidata[0]+(itidata[1]<<8);
skouki 1:697097e548f9 132
skouki 1:697097e548f9 133 if(X_ <= -1000)motor[2].setSpeed(-0.2);
skouki 1:697097e548f9 134 else motor[2].setSpeed(0.0);
skouki 1:697097e548f9 135
skouki 1:697097e548f9 136 }
skouki 0:f68b460de813 137 int main()
skouki 0:f68b460de813 138 {
skouki 0:f68b460de813 139 sita_s.setHeaders('A','Z');
skouki 0:f68b460de813 140 sita_s.startReceive(1);
skouki 0:f68b460de813 141 ue_s.setHeaders('A','Z');
skouki 0:f68b460de813 142 ue_s.startReceive(5);
skouki 1:697097e548f9 143 /*
skouki 1:697097e548f9 144 IR0.startAveraging(5);
skouki 1:697097e548f9 145 IR1.startAveraging(5);
skouki 1:697097e548f9 146 IR2.startAveraging(5);*/
skouki 1:697097e548f9 147
skouki 1:697097e548f9 148 pid.setInputLimits(-10000.0,10000.0);
skouki 1:697097e548f9 149 pid.setOutputLimits(-1.0,1.0);
skouki 1:697097e548f9 150 pid.setBias(0);
skouki 1:697097e548f9 151 pid.setMode(1);
skouki 1:697097e548f9 152 pid.setSetPoint(0.0);
skouki 1:697097e548f9 153
skouki 1:697097e548f9 154 field = mode_r_b.read();
skouki 1:697097e548f9 155 debug_led_1 = field;
skouki 1:697097e548f9 156 debug_led_2 = !field;
skouki 0:f68b460de813 157 while(1)
skouki 0:f68b460de813 158 {
skouki 1:697097e548f9 159 for(int i = 0;i < 2;i++)power[i] = 0.0;
skouki 0:f68b460de813 160 debug_led_0 = !debug_led_0;
skouki 1:697097e548f9 161 //ir_checker();
skouki 0:f68b460de813 162 to_ue();
skouki 0:f68b460de813 163 to_sita();
skouki 1:697097e548f9 164 iti_checker();
skouki 1:697097e548f9 165 if(start.read() == 0)mode = 1;
skouki 0:f68b460de813 166 if(mode==1&&s_mode ==2)mode = 2;
skouki 1:697097e548f9 167 if(mode == 2&&(u_mode == 0xff || s_mode == 0xff))mode = 0xff - 1;
skouki 1:697097e548f9 168 if((mode == 0xff - 1)&&u_mode == 0xff && s_mode == 0xff)mode = 3;
skouki 1:697097e548f9 169
skouki 1:697097e548f9 170
skouki 1:697097e548f9 171 if(mode == 3&&u_mode == 3 &&s_mode == 3)syoukou();
skouki 1:697097e548f9 172 if(mode == 3&&u_mode == 3 &&s_mode == 4)syoukou();
skouki 1:697097e548f9 173 if(mode == 3&&u_mode == 4 &&s_mode == 3)syoukou();
skouki 1:697097e548f9 174 if(mode == 3&&u_mode == 4 &&s_mode == 4)syoukou();
skouki 1:697097e548f9 175
skouki 1:697097e548f9 176 if(mode == 3&&u_mode == 4 &&s_mode == 4&&!end_flag)mode = 5;
skouki 1:697097e548f9 177
skouki 1:697097e548f9 178 if(mode == 1){
skouki 1:697097e548f9 179 if(wheel_1.getPulses() <= 1000)power[0] += 0.5;
skouki 1:697097e548f9 180 else power[0] = 0;
skouki 1:697097e548f9 181 if(wheel_2.getPulses() <= 1000)power[1] += 0.5;
skouki 1:697097e548f9 182 else power[1] = 0;
skouki 1:697097e548f9 183 }
skouki 1:697097e548f9 184 /*
skouki 1:697097e548f9 185 if(u_mode == 6&&s_mode==6){
skouki 1:697097e548f9 186 if((height_point-4000) < wheel_1.getPulses())power[0] -= 0.6;
skouki 1:697097e548f9 187 else power[0] = 0.0;
skouki 1:697097e548f9 188 if((height_point-4000) < wheel_2.getPulses())power[1] -= 0.6;
skouki 1:697097e548f9 189 else power[1] = 0.0;
skouki 1:697097e548f9 190 }
skouki 1:697097e548f9 191 */
skouki 1:697097e548f9 192
skouki 1:697097e548f9 193 motor[0].setSpeed(power[0]);
skouki 1:697097e548f9 194 motor[1].setSpeed(power[1]);
skouki 1:697097e548f9 195
skouki 1:697097e548f9 196
skouki 1:697097e548f9 197 pc.printf("%d,%d,%d,%d,%.2f,%.2f,%d\n\r",s_mode,u_mode,wheel_1.getPulses(),wheel_2.getPulses(),power[0],power[1],mode_r_b.read());
skouki 1:697097e548f9 198 //pc.printf("%d\n\r",data_a);
skouki 1:697097e548f9 199
skouki 0:f68b460de813 200
skouki 0:f68b460de813 201
skouki 0:f68b460de813 202 }
skouki 0:f68b460de813 203
skouki 0:f68b460de813 204
skouki 0:f68b460de813 205 return 0;
skouki 0:f68b460de813 206 }