aa
Dependencies: HMC6352 PID mbed
main.cpp
- Committer:
- akudohune
- Date:
- 2013-06-19
- Revision:
- 0:bde8ed56b164
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); } }