sasasa
Dependencies: HMC6352 PID eeprom mbed
Fork of ver1_2_2_1 by
main.cpp
- Committer:
- yusuke_robocup
- Date:
- 2013-04-18
- Revision:
- 3:b4fb2b5365a7
- Parent:
- 2:09fabba6c00d
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); } }