File content as of revision 0:2eebd0301c66:

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



void PidUpdate()
{   
    static uint8_t Fflag = 0;
    
    inputPID = (((int)(compass.sample() - ((standard + standTu) * 10.0) + 5400.0) % 3600) / 10.0);        
    
    //pc.printf("%f\n",timer1.read());
    pid.setProcessValue(inputPID);
    //timer1.reset();
    
    compassPID = -(pid.compute());
    
    if(!Fflag){
        Fflag = 1;
        compassPID = 0;
    }
    //pc.printf("%f\n",standard);
    //pc.printf("%d\n",diff);
    //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
    //pc.printf("%d\n",(int)compassPID);
    //pc.printf("%d\t%d\n",Distance,direction);
    //pc.printf("%d\t%d\t%d\t%d\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]);
}

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] = (int)motVal[i];
    }
}

/***********  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());
}
/*
void Rx_interrupt()
{
    if(driver.readable()){
        //pc.printf("%d\n",driver.getc());
    }
}*/


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


void init()
{
    int scanfSuccess;
    int printfSuccess;
    int closeSuccess;
    int close2Success;
    FILE *fp;
    
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    
    StartButton.mode(PullUp);
    CalibEnterButton.mode(PullUp);
    CalibExitButton.mode(PullUp);
    EEPROMButton.mode(PullUp);
    
    driver.baud(BAUD_RATE);
    device2.baud(BAUD_RATE2);
    wait_ms(MOTDRIVER_WAIT);///////////////////////////////////////////////////////////////
    driver.printf("1F0002F0003F0004F000\r\n"); 
    device2.printf("START");
    
    driver.attach(&Tx_interrupt, Serial::TxIrq);
    //driver.attach(&Rx_interrupt, Serial::RxIrq);
    device2.attach(&dev_rx,Serial::RxIrq);
    device2.attach(&dev_tx,Serial::TxIrq);
    
    mbedleds = 1;
    
    while(StartButton){
        if(!CalibEnterButton){
            mbedleds = 2;
            compass.setCalibrationMode(ENTER);
            while(CalibExitButton);
            compass.setCalibrationMode(EXIT);
            wait(BUT_WAIT);
            mbedleds = 4;
         }
         
         if(!EEPROMButton){
            fp = fopen("/local/out.txt", "r");
            if(fp == NULL){
                wait(BUT_WAIT);
                mbedleds = 3;
            }else{
                scanfSuccess = fscanf(fp, "%lf",&standard);
                if(scanfSuccess == EOF){
                    wait(BUT_WAIT);
                    mbedleds = 7;
                }else{
                    closeSuccess = fclose(fp);
                    if(closeSuccess == EOF){
                        wait(BUT_WAIT);
                        mbedleds = 15;
                    }else{
                        wait(BUT_WAIT);
                        mbedleds = 8;
                    }
                }
             }
         }
         
         if(!CalibExitButton){
            standard = compass.sample() / 10.0;
            
            fp = fopen("/local/out.txt", "w");
            if(fp == NULL){
                wait(BUT_WAIT);
                mbedleds = 3;
            }else{
                printfSuccess = fprintf(fp, "%f",standard);
                if(printfSuccess == EOF){
                    wait(BUT_WAIT);
                    mbedleds = 7;
                }else{
                    close2Success = fclose(fp);
                    if(close2Success == EOF){
                        wait(BUT_WAIT);
                        mbedleds = 15;
                    }else{
                        wait(BUT_WAIT);
                        mbedleds = 4;
                    }
                }
             }
        }
    }
    
    mbedleds = 0;
    
    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(&PidUpdate, 0.06);
    
    //timer1.start();
    //Ctimer.start();
}

int main()
{
    int vx=0,vy=0,vs=0;
    
    init();
           
    while(1){
        
        hold_flag = 0;
        vx = 0;
        vy = 0;
        vs = (int)compassPID;
        
        
        /*************** Change state **************************/
        
        if(state == HOLD){
            if(FRONT_SONIC < 100)hold_flag = 1;
            
            if(Distance > 140){
                mbedleds = 1;
                state = HOME_WAIT;
            }else if(!((direction == 0) || (direction == 15) || (direction == 1))){
                mbedleds = 4;
                state = DIFFENCE;
            }else if(!(Distance <= 30)){
                mbedleds = 4;
                state = DIFFENCE;
            }
        }else{
            standTu = 0;
            if(state == DIFFENCE){
                
                if((direction == 0) && (Distance <= 10) && (IR_found)){
                    mbedleds = 8;
                    state = HOLD;
                }else if((Distance <= 140) && (IR_found)){
                    mbedleds = 4;
                    state = DIFFENCE;
                }else{
                    if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
                        if(IR_found){
                            mbedleds = 4;
                            state = DIFFENCE;
                        }
                    }else if(diff > 15){
                         mbedleds = 4;
                         state = DIFFENCE;
                    }else{
                        mbedleds = 1;
                        state = HOME_WAIT;
                    }
                }
                
            }else{  
                if((direction == 0) && (Distance <= 10) && (IR_found)){
                    mbedleds = 8;
                    state = HOLD;
                }else if((Distance <= 120) && (IR_found)){
                    mbedleds = 4;
                    state = DIFFENCE;
                }else{
                    if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
                        if(IR_found){
                            mbedleds = 4;
                            state = DIFFENCE;
                        }       
                    }else if(diff > 15){
                         mbedleds = 4;
                         state = DIFFENCE;
                    }else if((Distance <= 120) && (IR_found)){
                        mbedleds = 2;
                        state = WARNING;
                    }else{
                        mbedleds = 1;
                        state = HOME_WAIT;
                    }
                }
            }
        }
        /*************** Change state **************************/
        //pc.printf("%f\t%f\t%f\t%f\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]);
        
        if(state == HOME_WAIT){
            if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 900.0)){
                if((LEFT_SONIC > 450.0) && (RIGHT_SONIC > 450.0)){
                    vx = 0;
                }else if(RIGHT_SONIC < 450.0){
                    vx = (int)((RIGHT_SONIC - 450.0) * 0.01 - 10.0);
                    if(vx < -15)vx = -15;
                }else if(LEFT_SONIC < 450.0){
                    vx = (int)((450.0 - LEFT_SONIC ) * 0.01 + 10.0);
                    if(vx > 15)vx = 15;
                }
                
                if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
                      if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
                        vy = 0;
                    }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
                        vy = 4;
                    }else{
                        vy = (int)(0.01 * (30.0 - BACK_SONIC) - 4);
                        if(vy < -10)vy = -10;
                    }
                }else{
                    if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){
                        vy = 0;
                    }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){
                        vy = 4;
                    }else{
                        vy = (int)(0.01 * (130.0 - BACK_SONIC) - 4);
                        if(vy < -10)vy = -10;
                    }
                }
            }else if((RIGHT_SONIC + LEFT_SONIC) <= 900.0){
                if(BACK_SONIC < 100.0){
                    if(RIGHT_SONIC > LEFT_SONIC){
                        vx = 10;
                        vy = 0;
                    }else{
                        vx = -10;
                        vy = 0;
                    }
                }else if(BACK_SONIC < 200.0){
                     if(RIGHT_SONIC > LEFT_SONIC){
                        vx = 10;
                        vy = -4;
                    }else{
                        vx = -10;
                        vy = -4;
                    }
                }else{
                    vx = 0;
                    vy = -10;
                }
            }else{
                if(BACK_SONIC > 500.0){
                    if(RIGHT_SONIC > LEFT_SONIC){
                        vx = 10;
                        vy = -5;
                    }else{                    
                        vx = -10;
                        vy = -5;
                    }
                }
            }
        }else if(state == DIFFENCE){
            if(direction == 6){
            
                if(BACK_SONIC < 110.0){
                    vy = 4;
                }else  if(BACK_SONIC > 500.0){
                    vy = -10;
                }else if(BACK_SONIC > 300.0){
                    vy = -7;
                }else if(BACK_SONIC > 140.0){
                    vy = -5;
                }else{
                    vy = 0;
                }
                     
                if(LEFT_SONIC < 700.0){
                    vx = 15;
                }else{
                    if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
                        vx = 10;
                        vy = -5;
                    }else{
                        vx = 10;
                        vy = -5;
                    }
                }
                
            }else if(direction == 10){
                /*
                if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
                      if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
                        vy = 0;
                    }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
                        vy = 4;
                    }else{
                        vy = (int)(0.01 * (30.0 - BACK_SONIC) - 4);
                        if(vy < -10)vy = -10;
                    }
                }else{
                    if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){
                        vy = 0;
                    }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){
                        vy = 4;
                    }else{
                        vy = (int)(0.01 * (130.0 - BACK_SONIC) - 4);
                        if(vy < -10)vy = -10;
                    }
                }
                     
                if(RIGHT_SONIC < 700.0){
                    vx = -15;
                }else{
                    if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
                        vx = -10;
                        vy = -5;
                    }else{
                        vx = -10;
                        vy = -5;
                    }
                }
                */
                if(BACK_SONIC < 110.0){
                    vy = 4;
                }else  if(BACK_SONIC > 500.0){
                    vy = -10;
                }else if(BACK_SONIC > 300.0){
                    vy = -7;
                }else if(BACK_SONIC > 140.0){
                    vy = -5;
                }else{
                    vy = 0;
                }
                     
                if(RIGHT_SONIC < 700.0){
                    vx = -15;
                }else{
                    if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
                        vx = -10;
                        vy = -5;
                    }else{
                        vx = -10;
                        vy = -5;
                    }
                }
            }else if(direction == 4){
            
                if(BACK_SONIC < 110.0){
                    vy = 4;
                }else  if(BACK_SONIC > 500.0){
                    vy = -10;
                }else if(BACK_SONIC > 300.0){
                    vy = -7;
                }else if(BACK_SONIC > 140.0){
                    vy = -5;
                }else{
                    vy = 0;
                }
                     
                if(LEFT_SONIC < 700.0){
                    vx = 15;
                }else{
                    if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
                        vx = 10;
                        vy = -5;
                    }else{
                        vx = 10;
                        vy = -5;
                    }
                }
                
            }else if(direction == 12){
            
                if(BACK_SONIC < 110.0){
                    vy = 4;
                }else  if(BACK_SONIC > 500.0){
                    vy = -10;
                }else if(BACK_SONIC > 300.0){
                    vy = -7;
                }else if(BACK_SONIC > 140.0){
                    vy = -5;
                }else{
                    vy = 0;
                }
                     
                if(RIGHT_SONIC < 700.0){
                    vx = -15;
                }else{
                    if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
                        vx = -10;
                        vy = -5;
                    }else{
                        vx = -10;
                        vy = -5;
                    }
                }
                
            }else if(direction == 8){
            
                if(LEFT_SONIC > RIGHT_SONIC){
                    vx = -10;
                }else{
                    vx = 10;
                }
                vy = -5;
                
            }else if(direction == 2){
                /*
                if((RIGHT_SONIC > 700.0) && (!(RIGHT_SONIC == PING_ERROR))){
                    vx = 30;
                }else{
                    vx = 20;
                } 
                */
                vx = 15;
                if(BACK_SONIC < 130.0){
                    //vy = 5;
                }
                /*
                if(BACK_SONIC > 140.0){
                    vy = -10;
                }else if(BACK_SONIC < 110.0){
                    vy = 5;
                }
                */
            }else if(direction == 14){
                /*
                if((LEFT_SONIC > 700.0) && (!(LEFT_SONIC == PING_ERROR))){
                    vx = -30;
                }else{
                    vx = -20;
                } 
                */
                vx = -15;   
                
                if(BACK_SONIC < 130.0){
                    //vy = 5;
                }
                
                /*
                if(BACK_SONIC > 140.0){
                    vy = -10;
                }else if(BACK_SONIC < 110.0){
                    vy = 5;
                }
                */
            }else if(direction == 1){
            
                vx = 10;    
                
            }else if(direction == 15){
            
                vx = -10;
                
            }else if(direction == 0){
            
                vx = 0;
                vy = 10;
                
            }else{//error
            
                vx = 0;
                vy = 0;
            
            }
        }else if(state == WARNING){
            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;
                }
            }
        }else if(state == HOLD){
            vy = 10;
            if(FRONT_SONIC < 100){
                if(RIGHT_SONIC < LEFT_SONIC){
                    if(BACK_SONIC > 500){
                        vy = 0;
                        vs = 3;
                    }
                }else{
                    if(BACK_SONIC > 500){
                        vy = 0;
                        vs = -3;
                    }
                }
            }else{
                if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 800.0)){
                    standTu = (RIGHT_SONIC - LEFT_SONIC) / 40.0;
                }else{
                    standTu = 0;
                }
            }
        }
        //vx = (int)(vx * 0.8);
        move(vx,vy,vs);
    }
}