sasasa
Dependencies: HMC6352 PID eeprom mbed
Fork of ver1_2_2_1 by
Diff: main.cpp
- Revision:
- 0:74bf4953c0d1
- Child:
- 1:89408fff7cc9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Mar 09 10:11:06 2013 +0000 @@ -0,0 +1,214 @@ +#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); + } +}