File content as of revision 1:89408fff7cc9:

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



void PidUpdata()
{    
    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};
    
    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] = 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()
{
    
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    StartButton.mode(PullUp);
    CalibEnterButton.mode(PullUp);
    CalibExitButton.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;
         }
    }
    
    standard = compass.sample() / 10.0;
    
    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(1);
    IR.attach(&IR_Position,0.04);
    ultrasonic.attach(&Ultrasonic, 0.05);
    driver.attach(&Tx_interrupt, Serial::TxIrq);
    //driver.attach(&Rx_interrupt, Serial::RxIrq);
    
    timer1.start();
    timer2.start();
}

int main()
{
    int vx=0,vy=0,vs=0;
    int state = HOME_WAIT;
    
    init();
           
    while(1) {
    
        vs = compassPID;
        //vx = 15;
        //vy = 10;
        
        /*
        if(IR_found){
            if((direction == 4) || (direction == 12)||(direction == 3) || (direction == 5) || (direction ==11) || (direction == 13)){
                vx = (int)(70*ball_sankaku[direction][0]);
                vy = (int)(70*ball_sankaku[direction][1]);
            }else{
                vx = (int)(70*ball_sankaku[direction][0]);
                vy = (int)(70*ball_sankaku[direction][1]);
            }
        }else{
            vx = 0;
            vy = 0;
        }
        */
        
        /*
        if(IR_found)state = DIFFENCE;
        else state = HOME_WAIT;
        */
        /*
        if(state == HOME_WAIT){
            if((ultrasonicVal[0] + ultrasonicVal[2]) < 6000){
                if(ultrasonicVal[0] > 3200){
                    vx = 15;
                    vy = 0;
                }else if(ultrasonicVal[2] > 3200){
                    vx = -15;
                    vy = 0;
                }else{
                    if(ultrasonicVal[1] > 800){
                        vx = 0;
                        vy = -15;
                    }else{
                        vx = 0;
                        vy = 0;
                    }   
                }
            }else{
                vx = 0;
                vy = 0;
            }
        }else if(state == DIFFENCE){
            if(ultrasonicVal[1] > 800){
                vx = 0;
                vy = -15;
            }else{
                if(distance <= 30){
                
                    if((direction == 1) || (direction == 2) || (direction == 3) || (direction == 4) || (direction == 5) || (direction == 6) || (direction == 7)){
                        vx = 15;
                
                    }else if((direction == 9) || (direction == 10) || (direction == 11) || (direction == 12) || (direction == 13) || (direction == 14) || (direction == 15)){
                        vx = -15;
            
                    }else{
                        vx = 0;
                    }
                }else{
                    
                }
            }     
        }
        */
        /*
        if(state == HOME_WAIT){
           
        }*/
        
            if((ultrasonicVal[0] + ultrasonicVal[2]) < 1050.0){
                if(ultrasonicVal[0] > 300.0){
                    vx = 15;
                    led3 = ON;
                    led4 = OFF;
                }else if(ultrasonicVal[2] > 300.0){
                    vx = -15;
                    led3 = ON;
                    led4 = OFF;
                }else{
                    led3 = OFF;
                    led4 = ON;
                    if(ultrasonicVal[1] > 200.0){
                        vy = -15;
                    }else if(ultrasonicVal[1] < 100.0){
                        vy = 15;
                    }else{
                        vy = 0;
                    }
                }
                led2 = ON;
            }else{
                vx = 0;
                vy = 0;
                led2 = OFF;
            }
            
        //pc.printf("%f,%f,%f\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2]);
            
        move(vx,vy,vs);
    }
}