ryo seki
/
ver1_2_2
test
main.cpp
- Committer:
- akudohune
- Date:
- 2013-03-09
- Revision:
- 0:74bf4953c0d1
File content as of revision 0:74bf4953c0d1:
#include <math.h> #include <sstream> #include "mbed.h" #include "HMC6352.h" #include "PID.h" #include "main.h" /*********** 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 PidUpdata() { if(standard < 180.0){ if((compass.sample() / 10.0) < standard){ inputPID = 180.0 -(standard - (compass.sample() / 10.0)); }else if((compass.sample() / 10.0) < 180.0 + standard){ inputPID = 180.0 +((compass.sample() / 10.0) - standard); }else{ inputPID = 180.0 - ((360.0 - (compass.sample() / 10.0)) + standard); } }else if(standard > 180.0){ if((compass.sample() / 10.0) > standard){ inputPID = 180.0 +((compass.sample() / 10.0) - standard); }else if((compass.sample() / 10.0) > standard - 180.0){ inputPID = 180.0 -(standard - (compass.sample() / 10.0)); }else{ inputPID = 180.0 + ((compass.sample() / 10.0) + (360.0 - standard)); } }else{ inputPID = compass.sample() / 10.0; } if(inputPID < 0){ inputPID *= -1; } //pc.printf("%f\n",timer1.read()); pid.setProcessValue(inputPID); //timer1.reset(); compassPID = -(pid.compute()); //pc.printf("%f\n",compassPID); } void move(int vx, int vy, int vs) { double motVal[MOT_NUM] = {0}; motVal[0] = (double)(((0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs)) * MOT1); motVal[1] = (double)(((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs)) * MOT2); motVal[2] = (double)(((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs)) * MOT3); motVal[3] = (double)(((0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs)) * MOT4); for(uint8_t i = 0;i < MOT_NUM;i++){ if(motVal[i] > 100)motVal[i] = 100; else if(motVal[i] < -100)motVal[i] = -100; 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()); } 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)(20*ball_sankaku[direction][0]); vy = (int)(20*ball_sankaku[direction][1]); }else{ vx = (int)(10*ball_sankaku[direction][0]); vy = (int)(10*ball_sankaku[direction][1]); } if(Distance <= 10){ vx *= -1; vy *= -1; } }else{ vx = 0; vy = 0; } */ /* if((ultrasonicVal[2] < 700) && (inputPID < 190) && (inputPID > 170))vx = -15; else vx = 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] > 700){ vx = 0; vy = -15; }else{ vx = 0; vy = 0; } } }else{ vx = 0; vy = 0; } }else if(state == DIFFENCE){ 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; } } */ move(vx,vy,vs); } }