aaaaa
Dependencies: HMC6352 PID QEI Servo mbed
main.cpp
- Committer:
- yusuke_robocup
- Date:
- 2013-09-30
- Revision:
- 0:1be472d79ae9
File content as of revision 0:1be472d79ae9:
#include "mbed.h" #include "PID.h" #include "QEI.h" #include "Servo.h" #include "HMC6352.h" #include "main.h" //guro-baru double vl = 0; double vc = 0; double vs = 1; int clwise = 0; int anclwise = 0; int enkoda1; int enkoda2; int process = 0; int vc_count = 0; int mode_comp = 0; int inputPID = 0; //guro-baru end void PidUpdate(){ static int state = WAIT,past_state = WAIT,next_state = WAIT; if(!mode_comp){ if(vl && !vs){ state = STRAIGHT; } if(vs){ state = TURN; } } if((state != past_state)){ mode_comp = 1; if(process == 0){ if(abs(enkoda1) > 180 && abs(enkoda1) < 240){ process++; } } if(process == 1){ if(state == STRAIGHT){ vl = 10; vs = 0; } if(state == TURN){ vl = 0; vs = 10; } if(abs(vc) < 60){ vc_count++; } if(vc_count > 2){ vc_count = 0; mode_comp = 0; process = 0; } } } enkoda1 = (int)(((double)wheel1.getPulses()/CYCLE) * 360.0); enkoda2 = (int)(((double)wheel2.getPulses()/CYCLE) * 360.0); if((!vs)&&(mode_comp == 0)){ pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses()); }else if((vs)&&(mode_comp == 0)){ pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses()); } if((!vs)&&(mode_comp)){ pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses()); }else if((vs)&&(mode_comp)){ pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses()); } vc = (int)pid.compute(); printf("vc:%lf\n",vc); double fut_R = 0.5,fut_L = 0.5; if(abs(vc) > 250){ vc = 0; } int vlR = 0; int vlL = 0; int vcR = vc; int vcL = vc; vlR = -vs; vlL = vs; vl *= 0.5; vc *= 0.5; vlR *= 0.4; vlL *= 0.4; vcR *= 0.6; vcL *= 0.6; if(!vs){ if(vl > 0){ fut_R = Convert_dekaruto((int)(vl - vc)); fut_L = Convert_dekaruto((int)(vl * 0.95 + vc)); } if(vl < 0){ vc *= -1; fut_R = Convert_dekaruto((int)(vl * 0.95 + vc)); fut_L = Convert_dekaruto((int)(vl - vc)); } }else{ if(vlR < 0){ vcR *= -1; } if(vlL < 0){ vcL *= -1; } if(vs > 0){ fut_R = Convert_dekaruto((int)(vlR * 0.9 + vcR)); fut_L = Convert_dekaruto((int)(vlL * 0.65 - vcL)); } if(vs < 0){ fut_R = Convert_dekaruto((int)(vlR * 0.65 - vcR)); fut_L = Convert_dekaruto((int)(vlL * 0.9+ vcL)); } } servoR = fut_R; servoL = fut_L; if(!mode_comp){ past_state = state; } } void CompassUpdate(){ inputPID = (((int)(compass.sample() - (/**/180 * 10.0) + 5400.0) % 3600) / 10.0); } double vssOut(){ double vss; vss = ((inputPID / 360.0 - 0.5) * 2.0 * 1000.0) * 1.0; if(abs(vss) < 10){ vss = 0; } vss *= 3.0; if((vss)&&(abs(vss) < 500)){ vss += (vss/abs(vss)) * 500; } if(abs(vss) > 1000){ vss = 1000 * (vss/abs(vss)); } return vss; } void move(int vll,int vss){ if(!swR){ wheel1.reset(); } if(!swL){ wheel2.reset(); } vl = vll; vs = vss; } #define PINR_THR 2000 int ping_button(int ping,int button){ static int continue_flag = 0; static int change_flag = 0; if(continue_flag == 0){ if(ping <= PINR_THR){ ping_t.start(); continue_flag = 1; } } if(continue_flag == 1){ //agatterutoki if(ping <= PINR_THR){ if(change_flag == 0){ if(ping_t.read_ms() >= 150){ button = !button; change_flag = 1; } } } //tatisagari if(ping >= (PINR_THR+200)){ ping_t.stop(); ping_t.reset(); continue_flag = 0; change_flag = 0; } } return button; } int main() { wait(3); timer2.start(); ping_t.start(); pid.setInputLimits(MINIMUM,MAXIMUM); pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); pid.setBias(PID_BIAS); pid.setMode(AUTO_MODE); pid.setSetPoint(REFERENCE); compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); compassUpdata.attach(&CompassUpdate, COMPASS_CYCLE); swR.mode(PullUp); swL.mode(PullUp); int setR = 0,setL = 0; int vll = 0,vss = 0; servoR = 0.595; servoL = 0.59; while(1){ if(!swR){ setR = 1; } if(!swL){ setL = 1; } if(setR){ servoR = 0.5; } if(setL){ servoL = 0.5; } if(setR && setL){ break; } wait(0.1); } wheel1.reset(); wheel2.reset(); pidUpdata.attach(&PidUpdate, PID_CYCLE); int button = 0; while(1) { vll = 0; vss = 0; Ultrasonic(); button = ping_button(ultrasonicVal[0],button); if(button){ vll = 400; led1 = 1; }else{ vll = -500; led1 = 0; } move(vll,vss); } }