File content as of revision 0:13ab960fc61f:

#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()
{
    float deviation = 0;
    
    
    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; //
    }
        
    
    //pid.setProcessValue(inputPID);
    
    deviation = (inputPID - 180.0);
    
    compassPID = ((deviation / 180.0) * 20.0 + ((deviation - lastData) / 2.0));
    
    if(compassPID > 100)compassPID = 100;
    else if(compassPID < -100)compassPID = -100;
    
    //pc.printf("%f\n",compassPID);
    
    lastData = deviation;
}



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));
    motVal[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs));
    motVal[2] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs));
    motVal[3] = (double)((0.5  * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs));
    
    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(-50.0, 50.0);
    pid.setBias(0.0);
    pid.setMode(AUTO_MODE);
    pid.setSetPoint(180.0);
    
    pidUpdata.attach(&PidUpdata, 0.06);
    //ultrasonic.attach(&Ultrasonic, 0.1);
    IR.attach(&IR_Position,0.04);
    driver.attach(&Tx_interrupt, Serial::TxIrq);
    //driver.attach(&Rx_interrupt, Serial::RxIrq);
    
    timer2.start();
}

int main()
{
    int vx=0,vy=0,vs=0;
    
    init();
           
    while(1) {
        vs = compassPID;
        
        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;
        }
        
        
        move(vx,vy,vs);
    }
}