aaaaa

Dependencies:   HMC6352 PID QEI Servo mbed

Revision:
0:1be472d79ae9
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Sep 30 09:01:37 2013 +0000
@@ -0,0 +1,293 @@
+#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); 
+    }
+}