File content as of revision 1:3b61675573b1:

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


void whiteliner()
{
    static int tmp[5] = {0};
    static int sum = 0;
    
    sum -= tmp[4];
    sum += whiteout_flag;
    tmp[4] = tmp[3];
    tmp[3] = tmp[2];
    tmp[2] = tmp[1];
    tmp[1] = tmp[0];
    tmp[0] = whiteout_flag;
    
    //return sum/5;
}

int hansya_x(int x)
{
    static int tmp[5] = {0};
    static int sum = 0;
    
    sum -= tmp[4];
    sum += x;
    tmp[4] = tmp[3];
    tmp[3] = tmp[2];
    tmp[2] = tmp[1];
    tmp[1] = tmp[0];
    tmp[0] = x;
    
    return sum/5;
}


int hansya_y(int y)
{
    static int tmp[5] = {0};
    static int sum = 0;
    
    sum -= tmp[4];
    sum += y;
    tmp[4] = tmp[3];
    tmp[3] = tmp[2];
    tmp[2] = tmp[1];
    tmp[1] = tmp[0];
    tmp[0] = y;
    
    return sum/5;
}


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

void move(int vxx, int vyy, int vss)
{
    double motVal[MOT_NUM] = {0};
    
    int angle = static_cast<int>( atan2( (double)vyy, (double)vxx ) * 180/PI + 360 ) % 360;
    
    double hosei1 = 1,hosei2 = 1,hosei3 = 1,hosei4 = 1;
    
    if(angle == 180){
        hosei1 = 1.3;
    }
    
    motVal[0] = (double)(((0.5 * vxx)  + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1)*hosei1;
    motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT2)*hosei2;
    motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3)*hosei3;
    motVal[3] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT4)*hosei4;
    
    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("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());
}

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);
    underIR.attach(&whiteliner, 0.05);
    
    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;
    int x_dista = 0,y_dista = 0,x_turn = 0,y_turn = 0;
    int state_off = NONE;
    int direction_av = 0;
    int direction_past = 0;
    int past_state_off = 0;
    int accelera_Distance = 0;
    uint8_t whiteFlag = 0;
    int save_vx = 0,save_vy = 0;
    
    int movein = 0;
    
    init();
           
    while(1) {
        
        vx=0;
        vy=0;
        
        //tuning = 0;
    
        x_dista = 0;
        y_dista = 0;
        x_turn  = 0;
        y_turn  = 0;
        //turn_flag = 0;
        
        vs = compassPID;
        
        //direction_av = moving_ave_5point(direction);
        
        
        
        for(uint8_t i = 0;i < 6;i++)
        {
            AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */
            irDistance[i] = ((adcIn.read_u16() >> 4) - stand[i]);
            if(irDistance[i] >= 30)
            {
                whiteFlag = 1;
                movein = 1;
                whiteout = 10;
            }        
            
            //pc.printf("%d\n",irDistance[0]);
        }       
        
        if(lineStateX == XNORMAL){
            if((LEFT_SONIC < 350) && (whiteFlag)){
                lineStateX = LEFT_OUT;
            }
            if((LEFT_SONIC < 350) && (RIGHT_SONIC < 100) && (whiteFlag)){
                lineStateX = LEFT_OUT;
            }
            
            if((RIGHT_SONIC < 350) && (whiteFlag)){    
                lineStateX = RIGHT_OUT;         
            }
            if((RIGHT_SONIC < 350) && (LEFT_SONIC < 100) && (whiteFlag)){
                lineStateX = RIGHT_OUT;
            }
            
        }else if(lineStateX == LEFT_OUT){
            /*
            if((LEFT_SONIC > 450) && (!whiteFlag)){
                lineStateX = XNORMAL;
                whiteFlag = 0;
            }
            */
            if((LEFT_SONIC > 450)){
                lineStateX = XNORMAL;
                whiteFlag = 0;
            }
        }else if(lineStateX == RIGHT_OUT){
            /*
            if((RIGHT_SONIC > 450) && (!whiteFlag)){
                lineStateX = XNORMAL;
                whiteFlag = 0;
            }
            */
            if((RIGHT_SONIC > 450)){
                lineStateX = XNORMAL;
                whiteFlag = 0;
            }
        }
        
        
        if(lineStateY == YNORMAL){
            if((FRONT_SONIC < 400) && (whiteFlag)){
                lineStateY = FRONT_OUT;
            }
            if((FRONT_SONIC < 400)&& (BACK_SONIC < 100) && (whiteFlag)){
                lineStateY = FRONT_OUT;
            }
            if((BACK_SONIC < 400) && (whiteFlag)){
                lineStateY = BACK_OUT;
            }
            if((BACK_SONIC < 400) && (FRONT_SONIC < 100) && (whiteFlag)){
                lineStateY = BACK_OUT;
            }
        }else if(lineStateY == FRONT_OUT){
            /*
            if((FRONT_SONIC > 500) && (!whiteFlag)){
                lineStateY = YNORMAL;
                whiteFlag = 0;
            }
            */
            if((FRONT_SONIC > 500)){
                lineStateY = YNORMAL;
                whiteFlag = 0;
            }
        }else if(lineStateY == BACK_OUT){
            /*
            if((BACK_SONIC > 500) && (!whiteFlag)){
                lineStateY = YNORMAL;
                whiteFlag = 0;
            }
            */
            if((BACK_SONIC > 500)){
                lineStateY = YNORMAL;
                whiteFlag = 0;
            }
        }
        
        
        if((state_off == ATTACK)&&(Distance == 10)){
            state = 1;
        }else{
            state = 0;
        }
           
        if(((direction == 0)||(direction == 1)||(direction == 15))){
            state_off = ATTACK;
        }
        if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance <= 90)){
            if((past_state_off != SNAKE)){
                accelera_Distance = Distance;    
            }
            state_off = SNAKE;
        }
        if(Distance >= 120){
            state_off = SEARCH;
        } 
        
        past_state_off = state_off;
        
        if(IR_found){
            if(state_off == SNAKE){
                if((Distance == 30)||(Distance == 90)){
                    x_dista = 12*ball_sankaku[direction][0];
                    y_dista = 12*ball_sankaku[direction][1];            
                    
                    x_turn = 18*(turn_sankaku[direction][0]);
                    y_turn = 18*(turn_sankaku[direction][1]);
                    
                    if((direction == 2)||(direction == 14)){
                        //x_turn *= 0.7;
                        y_turn *= 0.7;
                    }
                    
                    if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
                        x_turn = 7*(turn_sankaku[direction][0]);
                        y_turn = 7*(turn_sankaku[direction][1]);
                        
                        x_dista = 15*ball_sankaku[direction][0];
                        y_dista = 10*ball_sankaku[direction][1];        
                    
                    }
                    
                    if(direction == 1){
                        vx = 15;
                        vy = 0;
                    }
                    
                    if(direction == 15){
                        vx = -15;
                        vy = 0;
                    }
                    
                    if(direction == 2){
                        vx = 20;
                        vy = -10;
                    }
                    
                    if(direction == 14){
                        vx = -20;
                        vy = -10;
                    }
                    
                }
                
                if(Distance == 10){
                    x_dista = 8*(-ball_sankaku[direction][0]);
                    y_dista = 8*(-ball_sankaku[direction][1]);            
                
                    x_turn = 22*(turn_sankaku[direction][0]);
                    y_turn = 22*(turn_sankaku[direction][1]);
                    
                    
                    if((direction == 2)||(direction == 14)){
                        y_turn *= 0.7;
                    }
                    
                    if(direction == 2){
                        vx = 20;
                        vy = -15;  
                    }
                    if(direction == 14){
                        vx = -20;
                        vy = -15;
                    }         
                }
                
                if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
                    x_dista = 0;
                    y_dista = 0;    
                }
                
                if(direction == 2){
                    vx = 20;
                    vy = -15;  
                }
                if(direction == 14){
                    vx = -20;
                    vy = -15;
                }         
                    
                vx = x_turn + x_dista;
                vy = y_turn + y_dista;
                
                /*
                if((accelera_Distance == 90)){
                    if(Distance == 10){
                        vx *= 0.3;
                        vy *= 0.3;
                    }    
                    
                    if(Distance == 30){
                        vx *= 0.4;
                        vy *= 0.4;
                    }  
                }*/
                /*
                if((accelera_Distance == 10)){
                    if((direction == 4)||(direction == 12)){
                        vx = 0;
                        vy = -10;
                    }   
                }*/
                
            }else if(state_off == ATTACK){
                if(direction == 0){
                    vx = 10*ball_sankaku[direction][0];
                    vy =20*ball_sankaku[direction][1];
                    /*
                    if(ultrasonicVal[1] < 380){
                        vy = 10;
                        vx = -20;
                    }  
                    
                    if(ultrasonicVal[3] < 380){
                        vy = 10;
                        vx = 20;
                    } */    
                }
                if(direction == 1){
                    vx = 15*1.3;
                    vy = 20*1.3;
                }
                if(direction == 15){
                    vx = -15*1.3;
                    vy = 20*1.3;  
                }
                if(direction == 2){
                    vx = 25;
                    vy = 0;
                }
                if(direction == 14){
                    vx = -25;
                    vy = 0;
                }
                
                if(Distance > 30){
                    if(direction == 2){
                        vx = 20;
                        vy = 10;
                    }
                    if(direction == 14){
                        vx = -20;
                        vy = 10;
                    }    
                }
                
            }else if(state_off == SEARCH){
                vx = 24*ball_sankaku[direction][0];
                vy = 24*ball_sankaku[direction][1];
                
                if(direction == 2){
                    vx = 20;  
                }
                if(direction == 14){
                    vx = -20;
                }          
                if(direction == 0){
                    vx = 20*ball_sankaku[direction][0];
                    vy = 15*ball_sankaku[direction][1];  
                }
                if(direction == 1){
                    vx = 20*ball_sankaku[direction][0];
                    vy = 10*ball_sankaku[direction][1];  
                }
                if(direction == 15){
                    vx = 20*ball_sankaku[direction][0];
                    vy = 10*ball_sankaku[direction][1];  
                }
                if(direction == 4){
                    vx = 0;
                    vy = -15;
                }
                if(direction == 12){
                    vx = 0;
                    vy = -15;
                }                         
            }
            
            if(direction == 2){
                vx = 20;
                vy = 0;
                  
            }
            if(direction == 14){
                vx = -20;
                vy = 0;
            } 
             
        }else{
            vx = 0;
            vy = 0;
        }
        
        vx *= 1.3* 0.8;
        vy *= 0.7 * 0.8;
        

         
        if(lineStateX == LEFT_OUT){    
            vx += 20;
        }else if(lineStateX == RIGHT_OUT){
            vx -= 20;
        }
        
        if(lineStateY == FRONT_OUT){
            vy -= 15;
        }else if(lineStateY == BACK_OUT){
            vy += 15;
        } 
        
        //vx *= 0.8;
        //vy *= 0.8;
        
        direction_past = direction;
    
        
        move(vx,vy,vs);
    }
}