File content as of revision 0:bde8ed56b164:

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



void PidUpdate()
{   
    static uint8_t Fflag = 0;
    
    pid.setSetPoint(((int)((REFERENCE + standTu) + 360) % 360) / 1.0); 
    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());
    
    if(!Fflag){
        Fflag = 1;
        compassPID = 0;
    }
    //pc.printf("%d\t%d\t%d\n",speed[0],speed[1],speed[2]);
    //pc.printf("standard = \t\t%f\n",standard);
    //pc.printf("%d\n",diff);
    //pc.printf("compass.sample = \t%f\n",compass.sample() / 1.0);
    //pc.printf("compassPID = \t\t%d\n",(int)compassPID);
    //pc.printf("inputPID = \t\t%f\n\n",inputPID);
    
    //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 IrDistanceUpdate()
{
    for(uint8_t i = 0;i < 6;i++)
    {
        AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */
        irDistance[i] = adcIn.read_u16() >> 4;
        //pc.printf("%d\n",irDistance[0]);
    }
}


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];
    }
}

int vector(double Amp,double degree,int xy)
{
    double result; 

    if(xy == X){
        result = Amp * cos(degree * PI / 180.0);
    }else if(xy == Y){
        result = Amp * sin(degree * PI / 180.0) * (2.0 / sqrt(3.0));
    }
    
    return (int)result;
}

/***********  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);
    
    irDistanceUpdata.attach(&IrDistanceUpdate, 0.1);
    
    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();
    
    for(uint8_t i = 0;i < 6;i++)
    {
        stand[i] = irDistance[i];
    }
    irDistanceUpdata.detach();
    
    pidUpdata.attach(&PidUpdate, PID_CYCLE);
    //irDistanceUpdata.attach(&IrDistanceUpdate, 0.1);
    //timer1.start();
}

int main()
{
    int vx=0,vy=0,vs=0;
    uint8_t whiteFlag = 0;
    
    init();
           
    while(1){
        whiteFlag = 0;
        hold_flag = 0;
        vx = 0;
        vy = 0;
        vs = (int)compassPID;
        //vs = 0;
        
        //move(vx,vy,vs);
        
        /*********************************************************************************************************
        **********************************************************************************************************
        ********************Change state *************************************************************************
        **********************************************************************************************************
        *********************************************************************************************************/
        for(uint8_t i = 2;i < 6;i++)
        {
            AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */
            irDistance[i] = ((adcIn.read_u16() >> 4) - stand[i]);
            if(irDistance[i] >= 100)
            {
                whiteFlag = 1;
            }
            //pc.printf("%d\n",irDistance[0]);
        }
        
        if(lineState == NORMAL){
            if((LEFT_SONIC < 350) && (whiteFlag)){
                lineState = LEFT_OUT;
            }else if((RIGHT_SONIC < 350) && (whiteFlag)){    
                lineState = RIGHT_OUT;         
            }else if((FRONT_SONIC < 400) && (whiteFlag)){
                lineState = FRONT_OUT;
            }else if((BACK_SONIC < 400) && (whiteFlag)){
                lineState = BACK_OUT;
            }
        }else if(lineState == LEFT_OUT){
            if((LEFT_SONIC > 450) && (!whiteFlag)){
                lineState = NORMAL;
            }
        }else if(lineState == RIGHT_OUT){
            if((RIGHT_SONIC > 450) && (!whiteFlag)){
                lineState = NORMAL;
            }
        }else if(lineState == FRONT_OUT){
            if((FRONT_SONIC > 500) && (!whiteFlag)){
                lineState = NORMAL;
            }
        }else if(lineState == BACK_OUT){
            if((BACK_SONIC > 500) && (!whiteFlag)){
                lineState = NORMAL;
            }
        }
        
        
        if(state == HOLD){
            if(FRONT_SONIC < 100)hold_flag = 1;
            
            if(Distance > 140){ //審判のボール持ち上げ判定
                state = HOME_WAIT;
            }else if(!((direction == 0) || (direction == 15) || (direction == 1) || (direction == 14) || (direction == 2))){//ボールを見失った
                state = DIFFENCE;
            }else if(!(Distance <= 40)){//ボールを見失った
                state = DIFFENCE;
            }
        }else{
            standTu = 0;
            if(state == DIFFENCE){
                if((direction == 0) && (Distance <= 20) && (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 <= 30) && (IR_found) && (!xbee)){
                    state = HOLD;
                }else if((Distance <= 150) && (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 *************************************************************************
        **********************************************************************************************************
        *********************************************************************************************************/
        
        //state = HOME_WAIT;
        if(state == HOME_WAIT){
            mbedleds = 1;
            /*
            if(((RIGHT_SONIC + LEFT_SONIC) < 1100.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 - 425.0) * 0.02 - 10.0);
                    if(vx < -15)vx = -15;
                }else if(LEFT_SONIC < 425.0){
                    vx = (int)((425.0 - LEFT_SONIC ) * 0.02 + 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 = 5;
                    }else{
                        vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
                        if(vy < -10)vy = -10;
                    }
                }else{
                    if((BACK_SONIC > 95.0) && (BACK_SONIC < 125.0)){
                        vy = 0;
                    }else if((BACK_SONIC <= 95.0) || (BACK_SONIC == PING_ERROR)){
                        vy = 5;
                    }else{
                        vy = (int)(0.015 * (110.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 = 15;
                        vy = 5;
                    }else{
                        vx = -15;
                        vy = 5;
                    }
                }else{
                    vx = 0;
                    vy = -10;
                }
            }else{
                if(BACK_SONIC > 500.0){
                    if(RIGHT_SONIC > LEFT_SONIC){
                        vx = 10;
                        vy = -10;
                    }else{                    
                        vx = -10;
                        vy = -10;
                    }
                }
            }
            */
        }else if(state == DIFFENCE){
            mbedleds = 4;
            if((direction == 6) || (direction == 4)){
                vx = 20;
            
                if(LEFT_SONIC < 500){
                    if((BACK_SONIC > 370) && (BACK_SONIC < 400)){
                        vy = 0;
                    }else if((BACK_SONIC <= 370) || (BACK_SONIC == PING_ERROR)){
                        vy = 5;
                    }else{
                        vy = (int)(0.015 * (400.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }else if(RIGHT_SONIC < 500){
                    vx = 15;
                    vy = -10;
                }else{
                    if((BACK_SONIC > 70) && (BACK_SONIC < 100)){
                        vy = 0;
                    }else if((BACK_SONIC <= 70) || (BACK_SONIC == PING_ERROR)){
                        vy = 5;
                    }else{
                        vy = (int)(0.015 * (100.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }
            }else if((direction == 10) || (direction == 12)){
                 vx = -20;
            
                if(RIGHT_SONIC < 500){
                    if((BACK_SONIC > 370) && (BACK_SONIC < 400)){
                        vy = 0;
                    }else if((BACK_SONIC <= 370) || (BACK_SONIC == PING_ERROR)){
                        vy = 5;
                    }else{
                        vy = (int)(0.015 * (400.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }else if(LEFT_SONIC < 500){
                    vx = -15;
                    vy = -10;
                }else{
                    if((BACK_SONIC > 70) && (BACK_SONIC < 100)){
                        vy = 0;
                    }else if((BACK_SONIC <= 70) || (BACK_SONIC == PING_ERROR)){
                        vy = 5;
                    }else{
                        vy = (int)(0.015 * (100.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }
                
            }else if(direction == 8){
            
                if(LEFT_SONIC > RIGHT_SONIC){
                    vx = -20;
                }else{
                    vx = 20;
                }
                if((RIGHT_SONIC < 500) || (LEFT_SONIC < 500)){
                    if(BACK_SONIC < 500){
                        vy = -4;
                    }else{
                        vy = (int)(0.015 * (500.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }else{
                    if(BACK_SONIC < 200){
                        vy = -4;
                    }else{
                        vy = (int)(0.015 * (200.0 - BACK_SONIC) - 4);
                        if(vy < -15)vy = -15;
                    }
                }
                
            }else if(direction == 2){
                
                vx = 25;
                vy = 0;     //0
                
            }else if(direction == 14){
                
                vx = -25;
                vy = 0;    //-4 
                
            }else if(direction == 1){
            
                
                vx = 20;
                vy = 0;     //0
                
                
            }else if(direction == 15){
            
                vx = -20;
                vy = 0;    //-3
               
            }else if(direction == 0){
            
                vx = 0;
                vy = 20;
                
            }else{//error
            
                vx = 0;
                vy = 0;
            
            }
        }else if(state == HOLD){
            mbedleds = 15;
            
            vy = 20;
            
            if(((RIGHT_SONIC + LEFT_SONIC) < 1800.0) && ((RIGHT_SONIC + LEFT_SONIC) > 1400.0)){
                standTu = (RIGHT_SONIC - LEFT_SONIC) / 25.0;
            }
        }
        
        if(lineState == NORMAL){
            //mbedleds = 1;
           
        }else if(lineState == LEFT_OUT){
            //mbedleds = 2;
            
            vx = 30;
        }else if(lineState == RIGHT_OUT){
            //mbedleds = 4;
            
            vx = -30;
        }else if(lineState == FRONT_OUT){
            //mbedleds = 8;
            
            vy = -40;
        }else if(lineState == BACK_OUT){
            //mbedleds = 12;
            
            vy = 40;
        }
        //vx = vector(10,45,X);
        //vy = vector(10,45,Y);
        //vx = 40;
        //vy = 0;
        //pc.printf("%d\t%d\n",vx,vy);
        
        //vy = -3;
        //vs = 0;
        //vx = 0; 
        //vx = 10;
        //vx = 25;
        //vy = 0;
        
        move(vx,vy,vs);
    }
}