File content as of revision 0:8215f0743d86:

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



void PidUpdata()
{    
    inputPID = (((int)(compass.sample() - (standard * 10.0) + 5400.0) % 3600) / 10.0);        
    
    //pc.printf("%f\n",timer1.read());
    pid.setProcessValue(inputPID);
    //timer1.reset();
    
    compassPID = -(pid.compute());
    //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
    //pc.printf("%f\n",compassPID);
}

void move(int vxx, int vyy, int vss)
{
    double motVal[MOT_NUM] = {0};
    
    motVal[0] = (double)(((0.5 * vxx)  + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1);
    motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT2);
    motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3);
    motVal[3] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT4);
    
    for(uint8_t i = 0 ; i < MOT_NUM ; i++){
        if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
        else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
        speed[i] = motVal[i];
    }
    //pc.printf("%s",StringFIN.c_str());
}

/***********  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 init()
{
    uint8_t initFlag = 0;
    FILE *fp;
    
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    
    StartButton.mode(PullUp);
    CalibEnterButton.mode(PullUp);
    CalibExitButton.mode(PullUp);
    EEPROMButton.mode(PullUp);
    
    driver.baud(BAUD_RATE);
    wait_ms(MOTDRIVER_WAIT);
    driver.printf("1F0002F0003F0004F000\r\n"); 
    device2.printf("START");
    
    device2.attach(&dev_rx,Serial::RxIrq);
    device2.attach(&dev_tx,Serial::TxIrq);
    
    led1 = ON;
    
    while(StartButton){
        if(!CalibEnterButton){
            led1 = OFF;
            led2 = ON;
            compass.setCalibrationMode(ENTER);
            while(CalibExitButton);
            compass.setCalibrationMode(EXIT);
            led2 = OFF;
            led3 = ON;
         }
         if(!EEPROMButton){
            led3 = OFF;
            led1 = OFF;
            led4 = ON;
            initFlag = 1;
            fp = fopen("/local/out.txt", "r");
            fscanf(fp, "%lf",&standard);
            fclose(fp);
         }
    }
    
    if(!initFlag){
        standard = compass.sample() / 10.0;
        fp = fopen("/local/out.txt", "w");
        fprintf(fp, "%f",standard);
        fclose(fp);
    }
    
    led1 = OFF;
    led3 = OFF;
    led4 = 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_ms(500);       //danger
    driver.attach(&Tx_interrupt, Serial::TxIrq);
    //driver.attach(&Rx_interrupt, Serial::RxIrq);
    
    //timer1.start();
}

int main()
{
    int vx=0,vy=0,vs=0;
    uint8_t flag = 0;
    uint8_t comp_flag6 = 0;
    uint8_t comp_flag10 = 0;
    
    init();
           
    while(1){
        //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
        //wait(0.1);
        //pc.printf("%d\t,%d\n",direction,Distance);
        
        //pc.printf("%d\n",distance);
        
        vx = 0;
        vy = 0;
        vs = compassPID;
        
        /*************** Change state **************************/
        
        if((Distance <= 30) && (IR_found) && (!flag)){
            state = DIFFENCE;
        }else{
            if((direction == 4) || (direction == 6)|| (direction == 8) || (direction == 10)|| (direction == 12)){
                state = DIFFENCE;       
            }else if((Distance <= 120) && (IR_found) && (!flag)){
                state = WARNING;
            }else{
                state = HOME_WAIT;
            }
        }
        
        if((IR_found) && (!flag)){
            state = WARNING;
        }else{
            state = HOME_WAIT;
        }
            
        /*************** Change state **************************/
        
        
        if(state == HOME_WAIT){
            comp_flag6 = 0;
            comp_flag10 = 0;
            if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 600.0)){
                if((BACK_SONIC > 115.0) && (BACK_SONIC < 125.0)){
                    vy = 0;
                    flag = 0;
                }else if(BACK_SONIC <= 115.0){
                    vy = 3;
                }else if(BACK_SONIC == PING_ERROR){
                    vy = 5;
                }else{
                    vy = (int)(0.05 * (120.0 - BACK_SONIC) - 4);
                    if(vy < -30)vy = -30;
                }
                
                if((LEFT_SONIC > 450.0) && (RIGHT_SONIC > 450.0)){
                    vx = 0;
                }else if(RIGHT_SONIC < 450.0){
                    vx = (int)((RIGHT_SONIC - 450.0) / 15.0 - 10.0);
                    if(vx < -30)vx = -30;
                }else if(LEFT_SONIC < 450.0){
                    vx = (int)((450.0 - LEFT_SONIC ) / 15.0 + 10.0);
                    if(vx > 30)vx = 30;
                }
            }else if((RIGHT_SONIC + LEFT_SONIC) <= 600.0){
                if(BACK_SONIC < 100.0){
                    if(RIGHT_SONIC > LEFT_SONIC){
                        vx = 5;
                        vy = 10;
                    }else{
                        vx = -5;
                        vy = 10;
                    }
                }else if(BACK_SONIC < 200.0){
                     if(RIGHT_SONIC > LEFT_SONIC){
                        vx = 20;
                        vy = 0;
                    }else{
                        vx = -20;
                        vy = 0;
                    }
                }
            }else{
                if(BACK_SONIC > 500.0){
                    if(RIGHT_SONIC > LEFT_SONIC){
                        vx = 25;
                        vy = -10;
                    }else{                    
                        vx = -25;
                        vy = -10;
                    }
                }
            }
        }else if(state == DIFFENCE){
            if(direction == 6){
                comp_flag10 = 0;
                if(comp_flag6){
                    if(BACK_SONIC == PING_ERROR){
                        
                    }else if(BACK_SONIC > 50.0){
                        vy = -10;
                    }else{
                        
                    }
                    
                    if(LEFT_SONIC < 500.0){
                        vy = -20;
                    }else if(LEFT_SONIC == PING_ERROR){
                        vx = 20;
                    }else{
                        vx = 20;
                        vy = -20;
                    }
                }else{
                    if(BACK_SONIC < 110.0){
                        vy = 5;
                    }else  if(BACK_SONIC > 500.0){
                        vy = -20;
                    }else if(BACK_SONIC > 300.0){
                        vy = -10;
                    }else if(BACK_SONIC > 140.0){
                        vy = -5;
                    }else{
                        vy = 0;
                    }
                     
                    if(LEFT_SONIC < 700.0){
                        vx = 20;
                    }else{
                        if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
                            comp_flag6 = 1;
                        }else{
                            vx = 5;
                            vy = -15;
                        }
                    }
                }
            }else if(direction == 10){
                comp_flag6 = 0;
                if(comp_flag10){
                    if(BACK_SONIC == PING_ERROR){
                        
                    }else if(BACK_SONIC > 50.0){
                        vy = -10;
                    }else{
                        
                    }
                    
                    if(RIGHT_SONIC < 500.0){
                        vy = -20;
                    }else if(RIGHT_SONIC == PING_ERROR){
                        vx = -20;
                    }else{
                        vx = -20;
                        vy = -20;
                    }
                }else{
                
                    if(BACK_SONIC < 110.0){
                        vy = 5;
                    }else  if(BACK_SONIC > 500.0){
                        vy = -20;
                    }else if(BACK_SONIC > 300.0){
                        vy = -10;
                    }else if(BACK_SONIC > 140.0){
                        vy = -5;
                    }else{
                        vy = 0;
                    }
                     
                    if(RIGHT_SONIC < 700.0){
                        vx = -20;
                    }else{
                        if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
                            comp_flag10 = 1;
                        }else{
                            vx = -5;
                            vy = -15;
                        }
                    }
                }  
                
            }else if(direction == 4){
                comp_flag10 = 0;
                if(comp_flag6){
                    if(BACK_SONIC == PING_ERROR){
                        
                    }else if(BACK_SONIC > 50.0){
                        vy = -10;
                    }else{
                        
                    }
                
                    if(LEFT_SONIC < 500.0){
                        vy = -20;
                    }else if(LEFT_SONIC == PING_ERROR){
                        vx = 20;
                    }else{
                        vx = 20;
                        vy = -20;
                    }
                }else{
                    if(BACK_SONIC < 110.0){
                        vy = 5;
                    }else  if(BACK_SONIC > 500.0){
                        vy = -20;
                    }else if(BACK_SONIC > 300.0){
                        vy = -10;
                    }else if(BACK_SONIC > 140.0){
                        vy = -5;
                    }else{
                        vy = 0;
                    }
                     
                    if(LEFT_SONIC < 700.0){
                        vx = 20;
                    }else{
                        if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
                            comp_flag6 = 1;
                        }else{
                            vx = 5;
                            vy = -15;
                        }
                    }
                }
            }else if(direction == 12){
                comp_flag6 = 0;
                if(comp_flag10){
                    if(BACK_SONIC == PING_ERROR){
                        
                    }else if(BACK_SONIC > 50.0){
                        vy = -10;
                    }else{
                        
                    }
                    
                    if(RIGHT_SONIC < 500.0){
                        vy = -20;
                    }else if(RIGHT_SONIC == PING_ERROR){
                        vx = -20;
                    }else{
                        vx = -20;
                        vy = -20;
                    }
                }else{
                    if(BACK_SONIC < 110.0){
                        vy = 5;
                    }else  if(BACK_SONIC > 500.0){
                        vy = -20;
                    }else if(BACK_SONIC > 300.0){
                        vy = -10;
                    }else if(BACK_SONIC > 140.0){
                        vy = -5;
                    }else{
                        vy = 0;
                    }
                     
                    if(RIGHT_SONIC < 700.0){
                        vx = -20;
                    }else{
                        if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
                            comp_flag10 = 1;
                        }else{
                            vx = -5;
                            vy = -15;
                        }
                    }
                }
            }else if(direction == 8){
                if(comp_flag6){
                    vx = -15;
                    vy = -20;
                }else if(comp_flag10){
                    vx = 15;
                    vy = -20;
                }else{
                    if(LEFT_SONIC > RIGHT_SONIC){
                        vx = -15;
                        vy = -20;
                    }else{
                        vx = 15;
                        vy = -20;
                    }
                }
            }else if((direction == 2) || (direction == 1)){
                comp_flag6 = 0;
                comp_flag10 = 0;
                if((RIGHT_SONIC > 700.0) && (!(RIGHT_SONIC == PING_ERROR))){
                    vx = 30;
                }else{
                    vx = 20;
                }       
                if(BACK_SONIC > 140.0){
                    vy = -10;
                }else if(BACK_SONIC < 110.0){
                    vy = 5;
                }
            }else if((direction == 14) || (direction == 15)){
                comp_flag6 = 0;
                comp_flag10 = 0;
                if((LEFT_SONIC > 700.0) && (!(LEFT_SONIC == PING_ERROR))){
                    vx = -30;
                }else{
                    vx = -20;
                }       
                if(BACK_SONIC > 140.0){
                    vy = -10;
                }else if(BACK_SONIC < 110.0){
                    vy = 5;
                }
            }else if(direction == 0){
                comp_flag6 = 0;
                comp_flag10 = 0;
                vx = 0;
                vy = 20;
            }else{
                comp_flag6 = 0;
                comp_flag10 = 0;
                vx = 0;
                vy = 0;
            }
        }else if(state == WARNING){
            comp_flag6 = 0;
            comp_flag10 = 0;
            if(direction == 0){
                vx = 0;
            }else if(direction == 1){
                vx = 15;
            }else if(direction == 2){
                vx = 20;
            }else if(direction == 14){
                vx = -20;
            }else if(direction == 15){
                vx = -15;
            }
            
            if((LEFT_SONIC > 320) && (RIGHT_SONIC > 320)){
                if(BACK_SONIC > 150.0){
                    vy = -5;
                }else if(BACK_SONIC < 110.0){
                    vy = 5;
                }else{
                    vy = 0;
                }
            }else{
                if(BACK_SONIC > 110.0){
                    vy = -5;
                }else if(BACK_SONIC < 70.0){
                    vy = 5;
                }else{
                    vy = 0;
                }
            }
        }
        move(vx,vy,vs);
        
    }
}