File content as of revision 0:74bf4953c0d1:

#include <math.h>
#include <sstream>
#include "mbed.h"
#include "HMC6352.h"
#include "PID.h"
#include "main.h"


/***********  Serial interrupt  ***********/

void Tx_interrupt()
{
    array(speed[0],speed[1],speed[2],speed[3]);
    driver.printf("%s",StringFIN.c_str());
    //pc.printf("%s",StringFIN.c_str());
    //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
}
/*
void Rx_interrupt()
{
    if(driver.readable()){
        //pc.printf("%d\n",driver.getc());
    }
}*/


/***********  Serial interrupt end **********/

void PidUpdata()
{    
    
    if(standard < 180.0){
        if((compass.sample() / 10.0) < standard){
            inputPID = 180.0 -(standard - (compass.sample() / 10.0));
        }else if((compass.sample() / 10.0) < 180.0 + standard){
            inputPID = 180.0 +((compass.sample() / 10.0) - standard);
        }else{
            inputPID = 180.0 - ((360.0 - (compass.sample() / 10.0)) + standard);
        }
    }else if(standard > 180.0){
        if((compass.sample() / 10.0) > standard){
            inputPID = 180.0 +((compass.sample() / 10.0) - standard);
        }else if((compass.sample() / 10.0) > standard - 180.0){
            inputPID = 180.0 -(standard - (compass.sample() / 10.0));
        }else{
            inputPID = 180.0 + ((compass.sample() / 10.0) + (360.0 - standard));
        }
    }else{
        inputPID = compass.sample() / 10.0;
    }
    
    if(inputPID < 0){
        inputPID *= -1;
    }
        
    //pc.printf("%f\n",timer1.read());
    pid.setProcessValue(inputPID);
    //timer1.reset();
    
    compassPID = -(pid.compute());
    
    //pc.printf("%f\n",compassPID);

}



void move(int vx, int vy, int vs)
{
    double motVal[MOT_NUM] = {0};
    
    motVal[0] = (double)(((0.5 * vx)  + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs)) * MOT1);
    motVal[1] = (double)(((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long *  vs)) * MOT2);
    motVal[2] = (double)(((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs)) * MOT3);
    motVal[3] = (double)(((0.5  * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long *  vs)) * MOT4);
    
    for(uint8_t i = 0;i < MOT_NUM;i++){
        if(motVal[i] > 100)motVal[i] = 100;
        else if(motVal[i] < -100)motVal[i] = -100;
        speed[i] = motVal[i];
    }
    /*
    pc.printf("speed1 = %d\n",speed[0]);
    pc.printf("speed2 = %d\n",speed[1]);
    pc.printf("speed3 = %d\n",speed[2]);
    pc.printf("speed4 = %d\n\n",speed[3]);      
   */
    ////pc.printf("%s",StringFIN.c_str());
}

void init()
{
    
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    StartButton.mode(PullUp);
    CalibEnterButton.mode(PullUp);
    CalibExitButton.mode(PullUp);
    driver.baud(BAUD_RATE);
    wait_ms(MOTDRIVER_WAIT);
    driver.printf("1F0002F0003F0004F000\r\n"); 
    
    led1 = ON;
    
    while(StartButton){
        if(!CalibEnterButton){
            led1 = OFF;
            led2 = ON;
            compass.setCalibrationMode(ENTER);
            while(CalibExitButton);
            compass.setCalibrationMode(EXIT);
            led2 = OFF;
            led3 = ON;
         }
    }
    
    standard = compass.sample() / 10.0;
    led1 = OFF;
    led3 = OFF;
    
    pid.setInputLimits(0.0, 360.0);
    pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
    pid.setBias(0.0);
    pid.setMode(AUTO_MODE);
    pid.setSetPoint(180.0);
    
    pidUpdata.attach(&PidUpdata, 0.06);
    wait(1);
    IR.attach(&IR_Position,0.04);
    ultrasonic.attach(&Ultrasonic, 0.05);
    driver.attach(&Tx_interrupt, Serial::TxIrq);
    //driver.attach(&Rx_interrupt, Serial::RxIrq);
    
    timer1.start();
    timer2.start();
}

int main()
{
    int vx=0,vy=0,vs=0;
    int state = HOME_WAIT;
    
    init();
           
    while(1) {
    
        vs = compassPID;
        //vx = 15;
        //vy = 10;
        /*
        if(IR_found){
            if((direction == 4) || (direction == 12)||(direction == 3) || (direction == 5) || (direction ==11) || (direction == 13)){
                vx = (int)(20*ball_sankaku[direction][0]);
                vy = (int)(20*ball_sankaku[direction][1]);
            }else{
                vx = (int)(10*ball_sankaku[direction][0]);
                vy = (int)(10*ball_sankaku[direction][1]);
            }
            
            if(Distance <= 10){
                vx *= -1;
                vy *= -1;
            }
        }else{
            vx = 0;
            vy = 0;
        }
        */
        /*
        if((ultrasonicVal[2] < 700) && (inputPID < 190) && (inputPID > 170))vx = -15;
        else vx = 0;
        */
        /*
        if(IR_found)state = DIFFENCE;
        else state = HOME_WAIT;
        
        
        if(state == HOME_WAIT){
            if((ultrasonicVal[0] + ultrasonicVal[2]) < 6000){
                if(ultrasonicVal[0] > 3200){
                    vx = 15;
                    vy = 0;
                }else if(ultrasonicVal[2] > 3200){
                    vx = -15;
                    vy = 0;
                }else{
                    if(ultrasonicVal[1] > 700){
                        vx = 0;
                        vy = -15;
                    }else{
                        vx = 0;
                        vy = 0;
                    }   
                }
            }else{
                vx = 0;
                vy = 0;
            }
        }else if(state == DIFFENCE){
            if((direction == 1) || (direction == 2) || (direction == 3) || (direction == 4) || (direction == 5) || (direction == 6) || (direction == 7)){
                vx = 15;
                
            }else if((direction == 9) || (direction == 10) || (direction == 11) || (direction == 12) || (direction == 13) || (direction == 14) || (direction == 15)){
                vx = -15;
            
            }else{
                vx = 0;
                
            }
        }  
        */
        
        move(vx,vy,vs);
    }
}