File content as of revision 0:4644bf6bca6a:

#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 myservo1(p21);
Servo myservo2(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;
}*/



int main() {
    
    
    
    
    //printf("test\n");
    
    timer2.start();
    interrupt.attach(&tic_sensor, 0.1/*sec*/);
    //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    //printf("test%d\n",com_val);
    
    while(1) {
    wait(0.1);
        
        
        printf("pid:%.5d\n", ultrasonicVal[0]);
        
        if((ultrasonicVal[0] < 1000)&&(ultrasonicVal[1] < 1000)){
            myservo1 = 0.5;
            myservo2 = 0.5;
        }else if((ultrasonicVal[0] < ultrasonicVal[1])&&(ultrasonicVal[0] < 1000)){
            myservo1 = 0.7 + proportional;
            myservo2 = 0.7;
        }else if((ultrasonicVal[0] > ultrasonicVal[1])&&(ultrasonicVal[1] < 1000)){
            myservo1 = 0.3;
            myservo2 = 0.3 + proportional;
        }
    }
}