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); 
    }
}