main.cpp

00001 #include "mbed.h"
00002 
00003 BusOut lmotor(PA_0,PA_1);
00004 BusOut rmotor(PB_4,PB_5);
00005 
00006 DigitalOut test_led(PA_7);
00007 
00008 AnalogOut rvset(PA_5);
00009 AnalogOut lvset(PA_4);
00010 
00011 AnalogIn gyro(PB_1);
00012 
00013 Ticker count_time;
00014 
00015 float timec = 0;
00016 const float Kp = 100;
00017 const float Kd = 0;
00018 uint16_t motor = 0;
00019 double deg = 0;
00020 double bdeg = 0;
00021 double sensor;
00022 float unti;
00023 uint16_t vset = 0;
00024 
00025 
00026 void isr(){
00027     timec += 0.1;
00028     //sensor = (double)((gyro.read() * 3.3)-1.35) / (0.67 * 1000.0);
00029     sensor = 180;
00030     bdeg = sensor; 
00031     deg += (double)(sensor * 0.1);
00032 
00033     motor = 10 * (0 - deg) - Kd * sensor;
00034     if(motor < 0){
00035         lmotor = 0b10;
00036         rmotor = 0b10;
00037     }
00038     else if(motor > 0){
00039         lmotor = 0b01;
00040         rmotor = 0b01;
00041     }
00042     else{
00043         lmotor = 0b00;
00044         rmotor = 0b00;
00045     }
00046 
00047 }
00048 
00049 void speed(void){
00050     uint16_t num = motor / 5000;
00051     rvset.write(num);
00052     lvset.write(num);
00053 }
00054 
00055 int main(){  
00056     wait(3);
00057     count_time.attach(&isr, 0.1);
00058     for(;;){
00059         unti = sensor;
00060         speed();
00061         wait(0.1);
00062         test_led = 1;
00063         printf("deg  :%lf\n",deg);
00064         printf("moter:%lf\n",motor);
00065         printf("deg/s:%f\n",((gyro.read()* 3.3)-1.42)/(0.67/1000));
00066         //printf("time:%f\n",timec);
00067     }
00068 }