File content as of revision 3:b4fb2b5365a7:

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


BusOut mbedleds(LED4,LED3,LED2,LED1);

void PidUpdata()
{   
    if(turn_flag){
        now_compass = (((int)(compass.sample() - (past_compass) + 5400.0) % 3600) / 10.0); 
    }else{ 
        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("%f\n",compassPID);

}

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 > 30)&&(angle < 80)){
        hosei2 = 0.7;
        hosei4 = 0.7;
    }
    
    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());
}

/***********  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;
    char *hozon;
    
    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"); 
    
    led1 = ON;
    
    while(StartButton){
        if(!CalibEnterButton){
            led1 = OFF;
            led2 = ON;
            compass.setCalibrationMode(ENTER);
            while(CalibExitButton);
            compass.setCalibrationMode(EXIT);
            led2 = OFF;
            led3 = ON;
         }
         if(!EEPROMButton){
            initFlag = 1;
            read_eeprom(hozon,(char *)&standard,sizeof(hozon));
         }
    }
    if(!initFlag){
        standard = compass.sample() / 10.0;
        write_eeprom((char *)&standard,hozon,sizeof((char *)&standard));
    }
    led1 = OFF;
    led3 = 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(0.3);
    IR.attach(&IR_Position,0.06);
    ultrasonic.attach(&Ultrasonic, 0.05);
    driver.attach(&Tx_interrupt, Serial::TxIrq);
    //driver.attach(&Rx_interrupt, Serial::RxIrq);
    
    timer1.start();
    timer2.start();
}

uint16_t moving_ave_5point(uint16_t data)
{
    static uint16_t tmp[5] = {0};
    static uint32_t sum = 0;
    uint8_t i;
    uint8_t count;
    
    sum -= tmp[4];
    sum += data;
    tmp[4] = tmp[3];
    tmp[3] = tmp[2];
    tmp[2] = tmp[1];
    tmp[1] = tmp[0];
    tmp[0] = data;
    
    return sum/5;
}


int main()
{
    int vx=0,vy=0,vs=0;
    int x_dista = 0,y_dista = 0,x_turn = 0,y_turn = 0;
    int state = NONE;
    int direction_av = 0;
    int direction_past = 0;
    
    init();
           
    while(1) {
        x_dista = 0;
        y_dista = 0;
        x_turn  = 0;
        y_turn  = 0;
        turn_flag = 0;
        
        vs = compassPID;
        //vs = 0;
        //past_compass = compass.sample() / 1.0;
        //float now_compass = 180.0;
        /*
        while(1){
            vx = 10;
            vy = 10;
            vs = compassPID;
            
            move(vx,vy,vs);
        }*/
        
        direction_av = moving_ave_5point(direction);
           
        if(direction_av == 0){
            state = ATTACK;
        }
        if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance <= 30)){
            state = SNAKE;
        }
        if(Distance >= 90){
            state = SEARCH;
        } 
        
        if(IR_found){
            if(state == SNAKE){
                if(Distance == 30){
                    x_dista = 20*ball_sankaku[direction][0];
                    y_dista = 20*ball_sankaku[direction][1];            
                    
                    x_turn = 10*(turn_sankaku[direction][0]);
                    y_turn = 10*(turn_sankaku[direction][1]);
                    
                    if((direction == 2)||(direction == 14)){
                        x_turn *= 0.7;
                        y_turn *= 0.7;
                    }
                }
                
                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){
                        vs = -3;  
                    }
                    if(direction == 14){
                        vs = 3;  
                    }
                    
                    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_dista = 0;
                    y_dista = 0;    
                }
                    
                vx = x_turn + x_dista;
                vy = y_turn + y_dista;
                
            }else if(state == ATTACK){
                if(direction == 0){
                    vx = 10*ball_sankaku[direction][0];
                    vy = 15*ball_sankaku[direction][1];  
                }
                if(direction == 1){
                    vx = 20*ball_sankaku[direction][0];
                    vy = 12*ball_sankaku[direction][1];
                    vs = -2;  
                }
                if(direction == 15){
                    vx = 20*ball_sankaku[direction][0];
                    vy = 12*ball_sankaku[direction][1];
                    vs = 2;  
                }
                if(direction == 2){
                    vx = 10*ball_sankaku[direction][0];
                    vy = 10*ball_sankaku[direction][1];
                }
                if(direction == 14){
                    vx = 10*ball_sankaku[direction][0];
                    vy = 10*ball_sankaku[direction][1];
                }
                
            }else if(state == SEARCH){
                vx = 15*ball_sankaku[direction][0];
                vy = 15*ball_sankaku[direction][1];
                
                if(direction == 2){
                    vx = 25*ball_sankaku[direction][0];
                    vy = 10*ball_sankaku[direction][1];
                    vs = -2;  
                }
                if(direction == 14){
                    vx = -25*ball_sankaku[direction][0];
                    vy = 10*ball_sankaku[direction][1];
                    //vs = 2;  
                }          
                if(direction == 0){
                    vx = 10*ball_sankaku[direction][0];
                    vy = 15*ball_sankaku[direction][1];  
                }
                if(direction == 1){
                    vx = 30*ball_sankaku[direction][0];
                    vy = 8*ball_sankaku[direction][1];  
                }
                if(direction == 15){
                    vx = 30*ball_sankaku[direction][0];
                    vy = 8*ball_sankaku[direction][1];  
                }               
            } 
        }else{
            vx = 0;
            vy = 0;
        }
        
        /*
        if((ultrasonicVal[0]<50)||(ultrasonicVal[1]<50)||(ultrasonicVal[3]<50)){
            vx = (int)((30-FULL)*ball_sankaku[direction][0]);
            vy = (int)((30-FULL)*ball_sankaku[direction][1]);
        }
        
        
        
        if(Distance == 10){
            mbedleds = 0xF;    
        }else{
            mbedleds = 0x0;
        }
        
        if((ultrasonicVal[0]<100)&&(ultrasonicVal[1]<100)&&(ultrasonicVal[2]<100)&&((direction == 0)||(direction == 1)||(direction == 15))){
            vx = 0;
            vy = 0;
            vs = 0;
            past_compass = compass.sample() / 1.0;
            float now_compass = 180.0;
            
            if(inputPID > 180){
                turn_flag = 1;
                while((now_compass > 180.0 - (SHINPUKU / 2.0))&&((direction == 0)||(direction == 1)||(direction == 15))){
                    vs = 10;
                    move(vx,vy,vs);
                }
                turn_flag = 0;
            }
            
            if(inputPID < 180){
                turn_flag = 1;
                while((now_compass < 180.0 + (SHINPUKU / 2.0))&&((direction == 0)||(direction == 1)||(direction == 15))){
                    vs = -10;
                    move(vx,vy,vs);
                }
                turn_flag = 0; 
            }          
        }*/
        
        if(state == SNAKE){
            mbedleds = 0xF;    
        }else{
            mbedleds = 0x0;
        }
                
        
        vx *= 1;
        vy *= 1;
        
        direction_past = direction;
        
        move(vx,vy,vs);
    }
}