main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "TA8429.h"
00004 #define CONTROL_CYCLE 0.05
00005 #define PWM_FREQUENCY 20000.0
00006 
00007 #define DEFAULT_CW_SPEED 8.0
00008 #define DEFAULT_CCW_SPEED -90.0
00009 #define MIN_SPEED 8.0
00010 #define MAX_SPEED 90.0
00011 #define REVOLVE_NUM 1
00012 
00013 #define Kp 0.01
00014 #define Ki 0.001
00015 #define Kd 0
00016 
00017 DigitalOut myled(LED1);
00018 TA8429 driver(p21,p22,PWM_FREQUENCY);
00019 QEI encoder(p18,p24,p23,1024,QEI::X4_ENCODING);
00020 Serial pc(USBTX,USBRX);
00021 DigitalOut ledCW(LED1);
00022 DigitalOut ledCCW(LED2);
00023 AnalogIn thermalSensor(p17);
00024 AnalogIn volume(p16);
00025 Ticker controlTicker;
00026 Ticker sensingTicker;
00027 int g_control_flag;
00028 
00029 void set_control_flag(){
00030     g_control_flag = 1;
00031 }
00032 
00033 float calc_revolve_speed(){
00034     float revolve_speed= 0;
00035     int current_pulses = encoder.getPulses();
00036     static int last_pulses = 0;
00037     revolve_speed = (float)(last_pulses-current_pulses)*360.0/((float)CONTROL_CYCLE*4096.0);
00038     last_pulses = current_pulses;
00039     return revolve_speed;
00040 }
00041 
00042 float PID_control(float desired_revolve_speed,float current_revolve_speed,int reset_flag){
00043     float e,output;
00044     static float integrate_e = 0;
00045     e = desired_revolve_speed-current_revolve_speed;
00046     output = Kp*e+Ki*integrate_e;
00047     integrate_e += e;
00048     if(reset_flag == 1){
00049         integrate_e = 0;
00050         output = 0;
00051     }
00052     return output;
00053 }
00054 
00055 void send_temperature(){
00056     float temperature;   
00057     temperature = (thermalSensor*3.3-0.6)*100.0;
00058     pc.printf("%f\r",temperature);
00059 }
00060 
00061 
00062 float convert_volume2speed(float min,float max){
00063     float speed;
00064     speed = (max-min)*volume.read()+min;
00065     return speed;
00066 }
00067 
00068 int main(){
00069     float CW_speed = DEFAULT_CW_SPEED;
00070     float CCW_speed = DEFAULT_CCW_SPEED;
00071     int revolve_num = REVOLVE_NUM;
00072     int mode = 0;
00073     float desired_revolve_speed = 0.0;
00074     float current_revolve_speed = 0.0;
00075     float output = 0.0;
00076     char received_char;
00077     pc.baud(115200);
00078     controlTicker.attach(&set_control_flag,CONTROL_CYCLE);
00079     sensingTicker.attach(&send_temperature,0.06);
00080     desired_revolve_speed = 10.0;
00081     while(1){
00082         if(pc.readable()){
00083             received_char = pc.getc();
00084                 switch(received_char){
00085                      case 'p':
00086                          driver.stop();
00087                          controlTicker.detach();
00088                          myled = 0;
00089                          break;
00090                      case 's':
00091                          controlTicker.attach(&set_control_flag,CONTROL_CYCLE);
00092                          myled = 1;
00093                          break;
00094                  }
00095                  
00096              }
00097              if(g_control_flag == 1){
00098                 switch(mode){
00099                     case 0:
00100                         desired_revolve_speed = CW_speed;
00101                         if(encoder.getPulses() < -4096*revolve_num){
00102                             mode = 1;
00103                         }
00104                         break;
00105                     case 1:
00106                         driver.stop();
00107                         PID_control(0,0,1);
00108                         wait(1);
00109                         mode = 2;
00110                         break;
00111                     case 2:
00112                         desired_revolve_speed = CCW_speed;
00113                         if(encoder.getPulses() > 0){
00114                             mode = 3;
00115                         }
00116                         break;
00117                     case 3:
00118                         driver.stop();
00119                         PID_control(0,0,1);
00120                         wait(1);
00121                         CW_speed = convert_volume2speed(MIN_SPEED,MAX_SPEED);
00122                         mode = 0;
00123                         break;
00124                     }
00125                 current_revolve_speed = calc_revolve_speed();
00126                 output = PID_control(desired_revolve_speed,current_revolve_speed,0);
00127                 if(output > 0){
00128                     driver.set_CW(output);
00129                 }else if(output < 0){
00130                     driver.set_CCW(-output);
00131                 }else{
00132                     driver.stop();
00133                 }
00134                 //pc.printf("%f\r",read_sensor());
00135                 //pc.printf("desired:%.2f current:%.2f output:%.2f\r\n",desired_revolve_speed,current_revolve_speed,output);
00136                 g_control_flag = 0;
00137             }
00138         }
00139     }
00140