File content as of revision 0:fb6510639aa4:

#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) + 9000.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;
    uint8_t MissFlag = 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);
    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);
    
    pid.setInputLimits(MINIMUM,MAXIMUM);
    pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
    pid.setBias(PID_BIAS);
    pid.setMode(AUTO_MODE);
    pid.setSetPoint(REFERENCE);
    
    Survtimer.start();
    
    mbedleds = 1;
    
    while(StartButton){
        MissFlag = 0;
        if(!CalibEnterButton){
            mbedleds = 2;
            compass.setCalibrationMode(ENTER);
            while(CalibExitButton);
            compass.setCalibrationMode(EXIT);
            wait(BUT_WAIT);
            mbedleds = 4;
         }
         
         if(!EEPROMButton){
            Survtimer.reset();
            fp = fopen("/local/out.txt", "r");
            if(fp == NULL){
                wait(BUT_WAIT);
                MissFlag = 1;
            }else{
                scanfSuccess = fscanf(fp, "%lf",&standard);
                if(scanfSuccess == EOF){
                    wait(BUT_WAIT);
                    MissFlag = 1;
                }else{
                    closeSuccess = fclose(fp);
                    if(closeSuccess == EOF){
                        wait(BUT_WAIT);
                        MissFlag = 1;
                    }else{
                        wait(BUT_WAIT);
                    }
                }
            }
            if((Survtimer.read() > (BUT_WAIT + 0.1)) || (MissFlag)){
                for(uint8_t i = 0;i < 2;i++){
                    mbedleds = 15;
                    wait(0.1);
                    mbedleds = 0;
                    wait(0.1);
                }
                mbedleds = 15;
            }else{
                mbedleds = 8;
            }
         }
         
         if(!CalibExitButton){
            Survtimer.reset();
         
            standard = compass.sample() / 10.0;
            
            fp = fopen("/local/out.txt", "w");
            if(fp == NULL){
                wait(BUT_WAIT);
                MissFlag = 1;
            }else{
                printfSuccess = fprintf(fp, "%f",standard);
                if(printfSuccess == EOF){
                    wait(BUT_WAIT);
                    MissFlag = 1;
                }else{
                    close2Success = fclose(fp);
                    if(close2Success == EOF){
                        wait(BUT_WAIT);
                        MissFlag = 1;
                    }else{
                        wait(BUT_WAIT);
                    }
                }
            }
            if((Survtimer.read() > (BUT_WAIT + 0.2)) || (MissFlag)){
                for(uint8_t i = 0;i < 4;i++){
                    mbedleds = 15;
                    wait(0.1);
                    mbedleds = 0;
                    wait(0.1);
                }
                mbedleds = 15;
            }else{
                mbedleds = 10;
            }
        }
    }
    
    mbedleds = 0;
    
    Survtimer.stop();
    
    pidUpdata.attach(&PidUpdate, PID_CYCLE);
    
    //timer1.start();
}

int main()
{
    int vx=0,vy=0,vs=0;
    
    init();
           
    while(1){
        
        hold_flag = 0;
        vx = 0;
        vy = 0;
        vs = (int)compassPID;
        
        //move(vx,vy,vs);
        
        /******************************************************
        *******************************************************
        ********************Change state **********************
        *******************************************************
        ******************************************************/
        
        if(state == HOLD){
            if(FRONT_SONIC < 100)hold_flag = 1;
            
            if(Distance > 140){ //審判のボール持ち上げ判定
                state = HOME_WAIT;
            }else if(!((direction == 0) || (direction == 15) || (direction == 1))){//ボールを見失った
                state = DIFFENCE;
            }else if(!(Distance <= 30)){//ボールを見失った
                state = DIFFENCE;
            }
        }else{
            standTu = 0;
            if(state == DIFFENCE){
                if((direction == 0) && (Distance <= 10) && (IR_found) && (!xbee)){
                    state = HOLD;
                }else if((Distance < 180) && (IR_found) && (!xbee)){
                    state = DIFFENCE;
                }else{
                    if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
                        if((IR_found) && (!xbee)){
                            state = DIFFENCE;
                        }
                    }else if((diff > 15) && (!xbee) && (IR_found)){
                         state = DIFFENCE;
                    }else{
                        state = HOME_WAIT;
                    }
                }
                
            }else{  
                if((direction == 0) && (Distance <= 10) && (IR_found) && (!xbee)){
                    state = HOLD;
                }else if((Distance <= 140) && (IR_found) && (!xbee)){
                    state = DIFFENCE;
                }else{
                    if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
                        if((IR_found) && (!xbee)){
                            state = DIFFENCE;
                        }       
                    }else if((diff > 15) && (!xbee) && (IR_found)){
                         state = DIFFENCE;
                    }else{
                        state = HOME_WAIT;
                    }
                }
            }
        }
        
        /******************************************************
        *******************************************************
        ********************Change state **********************
        *******************************************************
        ******************************************************/
        
        if(state == HOME_WAIT){
            mbedleds = 1;
            if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 850.0)){
                if((LEFT_SONIC > 425.0) && (RIGHT_SONIC > 425.0)){
                    vx = 0;
                }else if(RIGHT_SONIC < 425.0){
                    vx = (int)((RIGHT_SONIC - 450.0) * 0.02 - 10.0);
                    if(vx < -20)vx = -20;
                }else if(LEFT_SONIC < 425.0){
                    vx = (int)((450.0 - LEFT_SONIC ) * 0.02 + 10.0);
                    if(vx > 20)vx = 20;
                }
                
                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) <= 850.0){
                if(BACK_SONIC < 200.0){
                    if(RIGHT_SONIC > LEFT_SONIC){
                        vx = 10;
                        vy = 3;
                    }else{
                        vx = -10;
                        vy = 3;
                    }
                }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){
            mbedleds = 4;
            if(direction == 6){
                vx = 15;
            
                if(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.015 * (30.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }else if(RIGHT_SONIC < 330.0){
                    vx = 10;
                    vy = -5;
                }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.015 * (130.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }
                
            }else if(direction == 10){
                vx = -15;
            
                if(RIGHT_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.015 * (30.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }else if(LEFT_SONIC < 330.0){
                    vx = -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.015 * (130.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }
            }else if(direction == 4){
            
                vx = 15;
            
                if(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.015 * (30.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }else if(RIGHT_SONIC < 330.0){
                    vx = 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.015 * (130.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }
                
                
            }else if(direction == 12){
            
                 vx = -15;
            
                if(RIGHT_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.015 * (30.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }else if(LEFT_SONIC < 330.0){
                    vx = -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.015 * (130.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }
                
            }else if(direction == 8){
            
                if(LEFT_SONIC > RIGHT_SONIC){
                    vx = -15;
                }else{
                    vx = 15;
                }
                if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
                    if(BACK_SONIC < 45.0){
                        vy = -5;
                    }else{
                        vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }else{
                    if(BACK_SONIC < 145.0){
                        vy = -5;
                    }else{
                        vy = (int)(0.015 * (130.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }
                
            }else if(direction == 2){
                
                if(BACK_SONIC < 150.0){
                    vx = 20;
                    vy = 0;
                }else{
                    vx = 12;
                    vy = 0;
                }
            }else if(direction == 14){
                
                if(BACK_SONIC < 150.0){
                    vx = -20;
                    vy = -4;
                }else{
                    vx = -12;
                    vy = -2;
                }    
                
            }else if(direction == 1){
            
                if(BACK_SONIC < 150.0){
                    vx = 15;
                    vy = 0;
                }else{
                    vx = 10;
                    vy = 0;
                } 
                
            }else if(direction == 15){
            
                if(BACK_SONIC < 150.0){
                    vx = -15;
                    vy = -3;
                }else{
                    vx = -10;
                    vy = -1;
                }
            }else if(direction == 0){
            
                vx = 0;
                vy = 10;
                
            }else{//error
            
                vx = 0;
                vy = 0;
            
            }
        }else if(state == WARNING){
            mbedleds = 2;
            if(direction == 0){
            
                vx = 0;
                
            }else if(direction == 1){
            
                vx = 10;
                
            }else if(direction == 2){
            
                vx = 15;
                
            }else if(direction == 14){
            
                vx = -15;
                
            }else if(direction == 15){
            
                vx = -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;
                }
            }
        }else if(state == HOLD){
            mbedleds = 8;
            vy = 15;
            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) / 30.0;
                }
            }
        }
       //move(vx,vy,vs);
    }
}