robocup

Dependencies:   HMC6352 PID mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }