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 servoR(p21);
00013 Servo servoL(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 #define Convert_dekaruto(a) ((a+100.0)/2.0/100.0)
00066 
00067 #define STRAIGHT 0.6;
00068 #define SPIN 0.4;
00069 
00070 //#define STRAIGHT 0.0;
00071 //#define SPIN 1.0;
00072 
00073 
00074 void move(int vl,int vs){
00075     double fut_R,fut_L,true_vs;
00076     
00077     true_vs = abs(vs)/SPIN;
00078     
00079     if(true_vs > 40){
00080         vl = 0;
00081         vs = 100*(vs/abs(vs));
00082     }
00083     
00084     fut_R = Convert_dekaruto((vl + vs));
00085     fut_L = Convert_dekaruto(-(vl - vs)*1.4);
00086  
00087     servoR = fut_R;
00088     servoL = fut_L;
00089        
00090     //printf("R:%lf   L:%lf\n",fut_R,fut_L);
00091     //printf("R:%d   L:%d\n",(vl + vs),-(vl - vs));
00092 }
00093 
00094 void PidUpdate()
00095 {   
00096     inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0);
00097     //printf("%lf\n",inputPID);      
00098 }
00099 
00100 double vsOut(){
00101     double vs;
00102     vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1;
00103     vs = vs * 8;
00104     
00105     if(vs/abs(vs) < 0){
00106         //vs *= 1.3;   
00107     }
00108     
00109     if(abs(vs) > 90)vs = 90*(vs/abs(vs));
00110     if(abs(vs) < 25) vs = 25*(vs/abs(vs));
00111     
00112     return vs;
00113 }
00114 
00115 
00116 int main() {
00117 
00118     wait(3);
00119 
00120     int vl;
00121     double vs;
00122     
00123     timer2.start();
00124     //interrupt.attach(&tic_sensor, 0.1/*sec*/);
00125     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00126     pidUpdata.attach(&PidUpdate, PID_CYCLE);
00127     //printf("test%d\n",com_val);
00128     
00129     while(1) {
00130         vl = -90;
00131         //vl = 0;
00132     
00133         vl = vl      * STRAIGHT;
00134         vs = vsOut() * SPIN;
00135         
00136         move(vl,(int)vs); 
00137     }
00138 }