main.cpp

00001 #include "mbed.h"
00002 //#include "QEI.h"
00003 #include "HMC6352.h"
00004 #include "Servo.h"
00005 #include "main.h"
00006 
00007 #define OUTRANGE_MAX    0.2//pid
00008 //#define ROTATE_PER_REVOLUTIONS  360//enko-da no bunkainou
00009 
00010 //QEI wheel(p23/*A*/, p24/*B*/, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
00011 HMC6352 compass(p28/*sda*/, p27/*scl*/);
00012 Servo myservo1(p21);
00013 Servo myservo2(p22);
00014 DigitalOut myled(LED1);
00015 Ticker interrupt;
00016 
00017 double PIDRead(uint8_t sensor, uint8_t target, uint8_t KP, uint8_t KI, uint8_t KD);
00018 
00019 double proportional = 0;
00020 uint16_t com_val = 0;
00021 
00022 
00023 void tic_sensor()
00024 {
00025     //int temp = (double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4);
00026     Ultrasonic();
00027     /*com_val = (compass.sample() / 10 + 180)%360;//180°を中間値にする
00028     //proportional = PIDRead(com_val, 180, 0.002, 0, 0);
00029     
00030     proportional = 0.002*(com_val-180);
00031     
00032     if(proportional > OUTRANGE_MAX){
00033         proportional = OUTRANGE_MAX;
00034     }else if(proportional < -OUTRANGE_MAX){
00035         proportional = -OUTRANGE_MAX;
00036     }*/
00037 }
00038 
00039 
00040 /*double PIDRead(uint8_t sensor, uint8_t target, uint8_t KP, uint8_t KI, uint8_t KD)
00041 {
00042     static double diff[2] = {0};
00043     double p,i,d = 0;
00044     double m;
00045     static int16_t integral = 0;
00046     
00047     diff[0] = diff[1];
00048     diff[1] = (double)(sensor - target);
00049     integral += (diff[0] + diff[1])/2;
00050     
00051     p = KP * diff[1];
00052     i = KI * integral;
00053     d = KD * (diff[1] - diff[0]);
00054     m = p+i+d;
00055       
00056     if(m > OUTRANGE_MAX){
00057         m = OUTRANGE_MAX;
00058     }else if(m < -OUTRANGE_MAX){
00059         m = -OUTRANGE_MAX;
00060     }
00061     
00062     return m;
00063 }*/
00064 
00065 
00066 
00067 int main() {
00068     
00069     
00070     
00071     
00072     //printf("test\n");
00073     
00074     timer2.start();
00075     interrupt.attach(&tic_sensor, 0.1/*sec*/);
00076     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00077     //printf("test%d\n",com_val);
00078     
00079     while(1) {
00080     wait(0.1);
00081         
00082         
00083       //  printf("pid:%.5d\n", ultrasonicVal[0]);
00084         printf("Heading is: %f\n", compass.sample() / 10.0);
00085 
00086         if((ultrasonicVal[0] < 1000)&&(ultrasonicVal[1] < 1000)){
00087             myservo1 = 0.5;
00088             myservo2 = 0.5;
00089         }else if((ultrasonicVal[0] < ultrasonicVal[1])&&(ultrasonicVal[0] < 1000)){
00090             myservo1 = 0.7 + proportional;
00091             myservo2 = 0.7;
00092         }else if((ultrasonicVal[0] > ultrasonicVal[1])&&(ultrasonicVal[1] < 1000)){
00093             myservo1 = 0.3;
00094             myservo2 = 0.3 + proportional;
00095         }
00096     }
00097 }