Ryo Ogata
ooooooooga
Dependencies: ColorSensor1 HMC6352 PID QEI Servo TextLCD mbed
Diff: main.cpp
- Revision:
- 0:e6bb0bcba274
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Oct 01 07:44:16 2013 +0000
@@ -0,0 +1,422 @@
+#include "mbed.h"
+#include "PID.h"
+#include "QEI.h"
+#include "Servo.h"
+#include "HMC6352.h"
+#include "ColorSensor.h"
+#include "TextLCD.h"
+
+#include "main.h"
+
+
+
+
+
+void tic_sensor()
+{
+ Ultrasonic();
+
+ colorUpdate();
+
+ /*lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+ */
+}
+
+
+
+
+
+////////////////////////////////////////移動関数//////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////
+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;
+}
+
+
+////////////////////////////////////////超音波センサの////////////////////////////////////////
+////////////////////////////////////////スイッチ的な関数//////////////////////////////////////
+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;
+}
+
+
+////////////////////////////////////////カラーセンサの////////////////////////////////////////
+////////////////////////////////////////補正プログラム////////////////////////////////////////
+void rivisedate()
+{
+ unsigned long red = 0,green = 0,blue =0;
+ static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
+
+ //最初の20回だけ平均を取る
+ for (int i=0;i<=20;i++){
+ color0.getRGB(R[0],G[0],B[0]);
+ red += R[0] ;
+ green += G[0] ;
+ blue += B[0] ;
+ //pc.printf(" %d %d\n",ptm(sum),sum);
+ }
+
+ rir = (double)green/ red ;
+ rib = (double)green/ blue ;
+}
+
+void colorUpdate()
+{
+ double colorSum[COLOR_NUM];
+ unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
+
+ color0.getRGB(R[0],G[0],B[0]);
+ color1.getRGB(R[1],G[1],B[1]);
+ color2.getRGB(R[2],G[2],B[2]);
+ /*color3.getRGB(R[3],G[3],B[3]);
+ color4.getRGB(R[4],G[4],B[4]);
+ color5.getRGB(R[5],G[5],B[5]);*/
+
+ for (int i=0; i<COLOR_NUM; i++){
+ colorSum[i] = R[i]*rir + G[i] + B[i]*rib ;
+ redp[i] = R[i]* rir * 100 / colorSum[i];
+ greenp[i] = G[i] * 100 / colorSum[i];
+ bluep[i] = B[i]* rib * 100 / colorSum[i];
+ }
+}
+
+
+////////////////////////////////////////ジャンププログラム////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////
+uint8_t jumpcondition()
+{
+ uint8_t threshold = 0, t[3] = {0};
+
+ //青から赤に0.5秒以内に反応したらジャンプ
+ for(int i=0; i<COLOR_NUM; i++){
+ if(bluep[i] >= B_THR){
+ color_t[i].reset();
+ color_t[i].start();
+ t[i] = 0;
+ }else if(redp[i] >= R_THR){
+ t[i] = color_t[i].read_ms();
+ }else{
+ t[i] = 0;
+ }
+
+ if((t[i] <= 500) && (t[i] != 0)){
+ threshold++;
+ }
+ }
+
+ return threshold;
+}
+
+void jumping(uint8_t threshold)
+{
+ //超音波でジャンプのタイミング合わせる
+ if(threshold >= 1){
+ jump_t.reset();
+ jump_t.start();
+ while(ultrasonicVal[0] < 1700){
+ led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0;
+ air[0] = 1; air[1] = 0;
+
+ if(jump_t.read_ms() > 1000)break;
+ }
+ led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1;
+ air[0] = 0; air[1] = 1;
+ wait(0.5);
+ }else{
+ led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
+ }
+}
+
+
+uint8_t robostop()
+{
+ if(bluep[1] >= B_THR){
+ return 1;
+ }else{
+ return 0;
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+int main() {
+ rivisedate();
+ 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);
+
+ air[0] = 0; air[1] = 1;
+
+ 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();
+
+ interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない
+ pidUpdata.attach(&PidUpdate, PID_CYCLE);
+
+ uint8_t button, state=0;
+
+ while(1) {
+ vll = 0;
+ vss = 0;
+
+ pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+ jumping(jumpcondition());
+
+
+
+ button = ping_button(ultrasonicVal[1],button);
+
+ if(button){
+ state = GO;
+ }else{
+ state = STOP;
+ }
+
+
+
+
+
+ if(state == GO){
+ vll = 700;//led[0] = 1; led[1] = 1;
+ }else if(state == STOP){
+ vll = -700;//led[0] = 0; led[1] = 0;
+ }
+
+ move(vll,vss);
+ }
+}