robocup
Dependencies: HMC6352 PID mbed
main.cpp
00001 #include <math.h> 00002 #include <sstream> 00003 #include "mbed.h" 00004 #include "HMC6352.h" 00005 #include "PID.h" 00006 #include "main.h" 00007 00008 00009 /*********** Serial interrupt ***********/ 00010 00011 void Tx_interrupt() 00012 { 00013 array(speed[0],speed[1],speed[2],speed[3]); 00014 driver.printf("%s",StringFIN.c_str()); 00015 //pc.printf("%s",StringFIN.c_str()); 00016 //pc.printf("compass.sample = %f\n",compass.sample() / 1.0); 00017 } 00018 /* 00019 void Rx_interrupt() 00020 { 00021 if(driver.readable()){ 00022 //pc.printf("%d\n",driver.getc()); 00023 } 00024 }*/ 00025 00026 00027 /*********** Serial interrupt end **********/ 00028 00029 void PidUpdata() 00030 { 00031 float deviation = 0; 00032 00033 00034 if(standard < 180.0){ 00035 if((compass.sample() / 10.0) < standard){ 00036 inputPID = 180.0 -(standard - (compass.sample() / 10.0)); 00037 }else if((compass.sample() / 10.0) < 180.0 + standard){ 00038 inputPID = 180.0 +((compass.sample() / 10.0) - standard); 00039 }else{ 00040 inputPID = 180.0 - ((360.0 - (compass.sample() / 10.0)) + standard); 00041 } 00042 }else if(standard > 180.0){ 00043 if((compass.sample() / 10.0) > standard){ 00044 inputPID = 180.0 +((compass.sample() / 10.0) - standard); 00045 }else if((compass.sample() / 10.0) > standard - 180.0){ 00046 inputPID = 180.0 -(standard - (compass.sample() / 10.0)); 00047 }else{ 00048 inputPID = 180.0 + ((compass.sample() / 10.0) + (360.0 - standard)); 00049 } 00050 }else{ 00051 inputPID = compass.sample() / 10.0; 00052 } 00053 00054 if(inputPID < 0){ 00055 inputPID *= -1; // 00056 } 00057 00058 00059 //pid.setProcessValue(inputPID); 00060 00061 deviation = (inputPID - 180.0); 00062 00063 compassPID = ((deviation / 180.0) * 20.0 + ((deviation - lastData) / 2.0)); 00064 00065 if(compassPID > 100)compassPID = 100; 00066 else if(compassPID < -100)compassPID = -100; 00067 00068 //pc.printf("%f\n",compassPID); 00069 00070 lastData = deviation; 00071 } 00072 00073 00074 00075 void move(int vx, int vy, int vs) 00076 { 00077 double motVal[MOT_NUM] = {0}; 00078 00079 motVal[0] = (double)((0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs)); 00080 motVal[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs)); 00081 motVal[2] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs)); 00082 motVal[3] = (double)((0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs)); 00083 00084 for(uint8_t i = 0;i < MOT_NUM;i++){ 00085 if(motVal[i] > 100)motVal[i] = 100; 00086 else if(motVal[i] < -100)motVal[i] = -100; 00087 speed[i] = motVal[i]; 00088 } 00089 /* 00090 pc.printf("speed1 = %d\n",speed[0]); 00091 pc.printf("speed2 = %d\n",speed[1]); 00092 pc.printf("speed3 = %d\n",speed[2]); 00093 pc.printf("speed4 = %d\n\n",speed[3]); 00094 */ 00095 ////pc.printf("%s",StringFIN.c_str()); 00096 } 00097 00098 void init() 00099 { 00100 00101 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); 00102 StartButton.mode(PullUp); 00103 CalibEnterButton.mode(PullUp); 00104 CalibExitButton.mode(PullUp); 00105 driver.baud(BAUD_RATE); 00106 wait_ms(MOTDRIVER_WAIT); 00107 driver.printf("1F0002F0003F0004F000\r\n"); 00108 00109 led1 = ON; 00110 00111 while(StartButton){ 00112 if(!CalibEnterButton){ 00113 led1 = OFF; 00114 led2 = ON; 00115 compass.setCalibrationMode(ENTER); 00116 while(CalibExitButton); 00117 compass.setCalibrationMode(EXIT); 00118 led2 = OFF; 00119 led3 = ON; 00120 } 00121 } 00122 00123 standard = compass.sample() / 10.0; 00124 led1 = OFF; 00125 led3 = OFF; 00126 00127 pid.setInputLimits(0.0, 360.0); 00128 pid.setOutputLimits(-50.0, 50.0); 00129 pid.setBias(0.0); 00130 pid.setMode(AUTO_MODE); 00131 pid.setSetPoint(180.0); 00132 00133 pidUpdata.attach(&PidUpdata, 0.06); 00134 //ultrasonic.attach(&Ultrasonic, 0.1); 00135 IR.attach(&IR_Position,0.04); 00136 driver.attach(&Tx_interrupt, Serial::TxIrq); 00137 //driver.attach(&Rx_interrupt, Serial::RxIrq); 00138 00139 timer2.start(); 00140 } 00141 00142 int main() 00143 { 00144 int vx=0,vy=0,vs=0; 00145 00146 init(); 00147 00148 while(1) { 00149 vs = compassPID; 00150 00151 if(IR_found){ 00152 if((direction == 4) || (direction == 12)||(direction == 3) || (direction == 5) || (direction ==11) || (direction == 13)){ 00153 vx = (int)(20*ball_sankaku[direction][0]); 00154 vy = (int)(20*ball_sankaku[direction][1]); 00155 }else{ 00156 vx = (int)(10*ball_sankaku[direction][0]); 00157 vy = (int)(10*ball_sankaku[direction][1]); 00158 } 00159 00160 if(Distance <= 10){ 00161 vx *= -1; 00162 vy *= -1; 00163 } 00164 }else{ 00165 vx = 0; 00166 vy = 0; 00167 } 00168 00169 00170 move(vx,vy,vs); 00171 } 00172 }
Generated on Sun Jul 17 2022 01:42:54 by 1.7.2