aa

Dependencies:   HMC6352 PID mbed

Revision:
0:e9b97faa3e37
diff -r 000000000000 -r e9b97faa3e37 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jun 17 00:12:40 2013 +0000
@@ -0,0 +1,565 @@
+#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)(((1.0  * vxx) + (0.0               * vyy) + (Long * vss)) * MOT3);
+    
+    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 = 0;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 <= 30) && (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 = 0;
+            /*
+            if(((RIGHT_SONIC + LEFT_SONIC) < 1650.0) && ((RIGHT_SONIC + LEFT_SONIC) > 1200.0)){
+                if((LEFT_SONIC > 600.0) && (RIGHT_SONIC > 600.0)){
+                    vx = 0;
+                }else if(RIGHT_SONIC < 600.0){
+                    vx = (int)((RIGHT_SONIC - 600.0) * 0.05 - 5.0);
+                    if(vx < -30)vx = -30;
+                }else if(LEFT_SONIC < 600.0){
+                    vx = (int)((600.0 - LEFT_SONIC ) * 0.05 + 5.0);
+                    if(vx > 30)vx = 30;
+                }
+                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.04 * (110.0 - BACK_SONIC) - 5.0);
+                    if(vy < -40)vy = -40;
+                }
+            }else if((RIGHT_SONIC + LEFT_SONIC) <= 1200.0){
+                if(BACK_SONIC < 200.0){
+                    if(RIGHT_SONIC > LEFT_SONIC){
+                        vx = 15;
+                        vy = 15;
+                    }else{
+                        vx = -15;
+                        vy = 15;
+                    }
+                }else{
+                    vx = 0;
+                    vy = -30;
+                }
+            }else{
+                if(BACK_SONIC > 500.0){
+                    if(RIGHT_SONIC > LEFT_SONIC){
+                        vx = 10;
+                        vy = -20;
+                    }else{                    
+                        vx = -10;
+                        vy = -20;
+                    }
+                }
+            }
+            */
+        }else if(state == DIFFENCE){
+           mbedleds = 1;
+            if((direction == 6) || (direction == 4)){
+                vx = 10;
+                vy = -30;
+                /*
+                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 = 5;
+                    }else{
+                        vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
+                        if(vy < -10)vy = -10;
+                    }
+                }else if(RIGHT_SONIC < 330.0){
+                    vx = 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((direction == 10) || (direction == 12)){
+                vx = -10;
+                vy = -30;
+                /*
+                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 = 5;
+                    }else{
+                        vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
+                        if(vy < -10)vy = -10;
+                    }
+                }else if(LEFT_SONIC < 330.0){
+                    vx = -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(direction == 8){
+            
+                if(LEFT_SONIC > RIGHT_SONIC){
+                    vx = -10;
+                }else{
+                    vx = 10;
+                }
+                vy = -40;
+                /*
+                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 < -10)vy = -10;
+                    }
+                }else{
+                    if(BACK_SONIC < 125.0){
+                        vy = -5;
+                    }else{
+                        vy = (int)(0.015 * (110.0 - BACK_SONIC) - 4);
+                        if(vy < -10)vy = -10;
+                    }
+                }
+                */
+            }else if(direction == 2){
+                
+                vx = 10;
+                vy = 5;     //0
+                
+            }else if(direction == 14){
+                
+                vx = -10;
+                vy = 5;    //-4 
+                
+            }else if(direction == 1){
+            
+                
+                vx = 5;
+                vy = 5;     //0
+                
+                
+            }else if(direction == 15){
+            
+                vx = -5;
+                vy = 5;    //-3
+               
+            }else if(direction == 0){
+            
+                vx = 0;
+                vy = 30;
+                
+            }else{//error
+            
+                vx = 0;
+                vy = 0;
+            
+            }
+        }else if(state == HOLD){
+            mbedleds = 15;
+            vx = 0;
+            vy = 40;
+            /*
+            if((FRONT_SONIC < 100) && (BACK_SONIC > 1300)){
+                if(RIGHT_SONIC < LEFT_SONIC){
+                    vy = 0;
+                    vx = 0;
+                    vs = 3;
+                }else{
+                    vy = 0;
+                    vx = 0;
+                    vs = -3;
+                }
+            }else{
+                if(((RIGHT_SONIC + LEFT_SONIC) < 1100.0) && ((RIGHT_SONIC + LEFT_SONIC) > 800.0)){
+                    standTu = (RIGHT_SONIC - LEFT_SONIC) / 25.0;
+                }
+            }
+            */
+        }
+        
+        if(lineState == NORMAL){
+            //mbedleds = 1;
+           
+        }else if(lineState == LEFT_OUT){
+            //mbedleds = 2;
+            
+            vx = 40;
+        }else if(lineState == RIGHT_OUT){
+            //mbedleds = 4;
+            
+            vx = -40;
+        }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 = -50; 
+        //vx = 10;
+        
+        move(vx,vy,vs);
+    }
+}
\ No newline at end of file