aa

Dependencies:   HMC6352 QEI Servo mbed

Fork of walkROBO by Ryo Ogata

main.cpp

Committer:
yusuke_robocup
Date:
2013-09-05
Revision:
2:955cdadf5ecc
Parent:
1:f465d89a26b0

File content as of revision 2:955cdadf5ecc:

#include "mbed.h"
//#include "QEI.h"
#include "HMC6352.h"
#include "Servo.h"
#include "main.h"

#define OUTRANGE_MAX    0.2//pid
//#define ROTATE_PER_REVOLUTIONS  360//enko-da no bunkainou

//QEI wheel(p23/*A*/, p24/*B*/, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
//HMC6352 compass(p28/*sda*/, p27/*scl*/);
Servo servoR(p21);
Servo servoL(p22);
DigitalOut myled(LED1);
Ticker interrupt;

double PIDRead(uint8_t sensor, uint8_t target, uint8_t KP, uint8_t KI, uint8_t KD);

double proportional = 0;
uint16_t com_val = 0;


void tic_sensor()
{
    //int temp = (double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4);
    Ultrasonic();
    /*com_val = (compass.sample() / 10 + 180)%360;//180°を中間値にする
    //proportional = PIDRead(com_val, 180, 0.002, 0, 0);
    
    proportional = 0.002*(com_val-180);
    
    if(proportional > OUTRANGE_MAX){
        proportional = OUTRANGE_MAX;
    }else if(proportional < -OUTRANGE_MAX){
        proportional = -OUTRANGE_MAX;
    }*/
}


/*double PIDRead(uint8_t sensor, uint8_t target, uint8_t KP, uint8_t KI, uint8_t KD)
{
    static double diff[2] = {0};
    double p,i,d = 0;
    double m;
    static int16_t integral = 0;
    
    diff[0] = diff[1];
    diff[1] = (double)(sensor - target);
    integral += (diff[0] + diff[1])/2;
    
    p = KP * diff[1];
    i = KI * integral;
    d = KD * (diff[1] - diff[0]);
    m = p+i+d;
      
    if(m > OUTRANGE_MAX){
        m = OUTRANGE_MAX;
    }else if(m < -OUTRANGE_MAX){
        m = -OUTRANGE_MAX;
    }
    
    return m;
}*/
 
#define Convert_dekaruto(a) ((a+100.0)/2.0/100.0)

#define STRAIGHT 0.6;
#define SPIN 0.4;

//#define STRAIGHT 0.0;
//#define SPIN 1.0;


void move(int vl,int vs){
    double fut_R,fut_L,true_vs;
    
    true_vs = abs(vs)/SPIN;
    
    if(true_vs > 40){
        vl = 0;
        vs = 100*(vs/abs(vs));
    }
    
    fut_R = Convert_dekaruto((vl + vs));
    fut_L = Convert_dekaruto(-(vl - vs)*1.4);
 
    servoR = fut_R;
    servoL = fut_L;
       
    //printf("R:%lf   L:%lf\n",fut_R,fut_L);
    //printf("R:%d   L:%d\n",(vl + vs),-(vl - vs));
}

void PidUpdate()
{   
    inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0);
    //printf("%lf\n",inputPID);      
}

double vsOut(){
    double vs;
    vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1;
    vs = vs * 8;
    
    if(vs/abs(vs) < 0){
        //vs *= 1.3;   
    }
    
    if(abs(vs) > 90)vs = 90*(vs/abs(vs));
    if(abs(vs) < 25) vs = 25*(vs/abs(vs));
    
    return vs;
}


int main() {

    wait(3);

    int vl;
    double vs;
    
    timer2.start();
    //interrupt.attach(&tic_sensor, 0.1/*sec*/);
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    pidUpdata.attach(&PidUpdate, PID_CYCLE);
    //printf("test%d\n",com_val);
    
    while(1) {
        vl = -90;
        //vl = 0;
    
        vl = vl      * STRAIGHT;
        vs = vsOut() * SPIN;
        
        move(vl,(int)vs); 
    }
}