Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 /* LAB DCMotor */
00002 #include "mbed.h"
00003  
00004 //****************************************************************************** Define
00005 //The number will be compiled as type "double" in default
00006 //Add a "f" after the number can make it compiled as type "float"
00007 #define Ts 0.01f    //period of timer1 (s)
00008 #define Servo_Period 20
00009 //****************************************************************************** End of Define
00010  
00011 //****************************************************************************** I/O
00012 //PWM
00013 Serial pc(USBTX, USBRX); // tx, rx
00014  
00015 //Dc motor
00016 PwmOut pwm1(D7);
00017 PwmOut pwm1n(D11);
00018 PwmOut pwm2(D8);
00019 PwmOut pwm2n(A3);
00020 PwmOut servo(A0);
00021 //Motor1 sensor
00022 InterruptIn HallA_1(A1);
00023 InterruptIn HallB_1(A2);
00024 //Motor2 sensor
00025 InterruptIn HallA_2(D13);
00026 InterruptIn HallB_2(D12);
00027  
00028 //LED
00029 DigitalOut led1(A4);
00030 DigitalOut led2(A5);
00031  
00032 //Timer Setting
00033 Ticker timer;
00034 //****************************************************************************** End of I/O
00035  
00036 //****************************************************************************** Functions
00037 void init_timer(void);
00038 void init_CN(void);
00039 void init_PWM(void);
00040 void timer_interrupt(void);
00041 void CN_interrupt(void);
00042 //****************************************************************************** End of Functions
00043  
00044 //****************************************************************************** Variables
00045 // Servo
00046 float servo_duty = 0.03; // 0.025~0.113(-90~+90) 0.069->0 degree
00047 // motor 1
00048 int8_t HallA_state_1 = 0;
00049 int8_t HallB_state_1 = 0;
00050 int8_t motor_state_1 = 0;
00051 int8_t motor_state_old_1 = 0;
00052 int count_1 = 0;
00053 float speed_1 = 0.0f;
00054 float v_ref_1 = -80.0f;
00055 float v_err_1 = 0.0f;
00056 float v_ierr_1 = 0.0f;
00057 float ctrl_output_1 = 0.0f;
00058 float pwm1_duty = 0.0f;
00059 //motor 2
00060 int8_t HallA_state_2 = 0;
00061 int8_t HallB_state_2 = 0;
00062 int8_t motor_state_2 = 0;
00063 int8_t motor_state_old_2 = 0;
00064 int count_2 = 0;
00065 float speed_2 = 0.0f;
00066 float v_ref_2 = 150.0f;
00067 float v_err_2 = 0.0f;
00068 float v_ierr_2 = 0.0f;
00069 float ctrl_output_2 = 0.0f;
00070 float pwm2_duty = 0.0f;
00071 //servo
00072 int i = 0;
00073 //****************************************************************************** End of Variables
00074  
00075 //****************************************************************************** Main
00076 int main()
00077 {
00078     init_timer();
00079     init_PWM();
00080     init_CN();
00081     while(1)
00082     {
00083         pc.printf("**************************\n");       
00084         pc.printf("speed1: %f, error: %f\n",speed_1,v_err_1);
00085         pc.printf("i_error: %f ctrl output: %f \n",v_ierr_1,ctrl_output_1);
00086         pc.printf("speed2: %f, error: %f\n",speed_2,v_err_2);
00087         pc.printf("i_error: %f ctrl output: %f \n",v_ierr_2,ctrl_output_2);
00088  
00089     }
00090 }
00091 //****************************************************************************** End of Main
00092  
00093 //****************************************************************************** timer_interrupt
00094 void timer_interrupt()
00095 {   
00096     // Motor1
00097     speed_1 = (float)count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
00098     count_1 = 0;
00099     // Code for PI controller //
00100     v_err_1 = v_ref_1 - speed_1;
00101     v_ierr_1 += (v_err_1*Ts);
00102     ctrl_output_1 = 0.01f*v_err_1+ 0.1f*v_ierr_1; 
00103     ///////////////////////////
00104     
00105     if(ctrl_output_1 >= 0.5f)ctrl_output_1 = 0.5f;
00106     else if(ctrl_output_1 <= -0.5f)ctrl_output_1 = -0.5f;
00107     pwm1_duty = ctrl_output_1 + 0.5f;
00108     pwm1.write(pwm1_duty);
00109     TIM1->CCER |= 0x4;
00110     
00111     // Motor2
00112     speed_2 = (float)count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
00113     count_2 = 0;
00114     // Code for PI controller //
00115     v_err_2 = v_ref_2 - speed_2;
00116     v_ierr_2 += (v_err_2*Ts);
00117     ctrl_output_2 = 0.001f*v_err_2+ 0.05f*v_ierr_2;
00118     ///////////////////////////      
00119     if(ctrl_output_2 >= 0.5f)ctrl_output_2 = 0.5f;
00120     else if(ctrl_output_2 <= -0.5f)ctrl_output_2 = -0.5f;
00121     pwm2_duty = ctrl_output_2 + 0.5f;
00122     pwm2.write(pwm2_duty);
00123     TIM1->CCER |= 0x40;
00124     
00125     if(v_ierr_1 > 5)
00126         v_ierr_1 = 0;
00127     if(v_ierr_2 > 8)
00128         v_ierr_2 = 0;
00129         
00130     //Servo
00131     if(i==100) 
00132     {
00133         if(servo_duty < 0.07f)
00134         {
00135             servo_duty = servo_duty+0.04f/6;
00136         }
00137         else
00138         {
00139             servo_duty = 0.07f;            
00140         }
00141         servo.write(servo_duty);
00142         i=0;
00143     }
00144     else
00145     {
00146         i++;
00147     }
00148 }
00149 //****************************************************************************** End of timer_interrupt
00150  
00151 //****************************************************************************** CN_interrupt
00152 void CN_interrupt()
00153 {
00154     // Motor1
00155     // Read the current status of hall sensor
00156     HallA_state_1 = HallA_1.read();
00157     HallB_state_1 = HallB_1.read();
00158      
00159    ///code for state determination///
00160     if(HallA_state_1 == 0 && HallB_state_1 == 0)
00161         motor_state_1 = 1;
00162     else if(HallA_state_1 == 0 && HallB_state_1 == 1)
00163         motor_state_1 = 2;
00164     else if(HallA_state_1 == 1 && HallB_state_1 == 1)
00165         motor_state_1 = 3;
00166     else if(HallA_state_1 == 1 && HallB_state_1 == 0)
00167         motor_state_1 = 4;
00168     
00169     if(motor_state_old_1 != 0)
00170     {
00171         if(motor_state_old_1 < motor_state_1)
00172             count_1 += 1;
00173         else if(motor_state_old_1 > motor_state_1)
00174             count_1 -= 1;
00175         if(motor_state_old_1 == 4 && motor_state_1 == 1)
00176             count_1 += 2;
00177         if(motor_state_old_1 == 1 && motor_state_1 == 4)
00178             count_1 -= 2;
00179     }
00180     motor_state_old_1 = motor_state_1;
00181     //////////////////////////////////
00182     
00183     //Forward
00184     //v1Count +1
00185     //Inverse
00186     //v1Count -1
00187         
00188     // Motor2
00189     // Read the current status of hall sensor
00190     HallA_state_2 = HallA_2.read();
00191     HallB_state_2 = HallB_2.read();
00192  
00193     ///code for state determination///
00194     if(HallA_state_2 == 0 && HallB_state_2 == 0)
00195         motor_state_2 = 1;
00196     else if(HallA_state_2 == 0 && HallB_state_2 == 1)
00197         motor_state_2 = 2;
00198     else if(HallA_state_2 == 1 && HallB_state_2 == 1)
00199         motor_state_2 = 3;
00200     else if(HallA_state_2 == 1 && HallB_state_2 == 0)
00201         motor_state_2 = 4;
00202     
00203     if(motor_state_old_2 != 0)
00204     {
00205         if(motor_state_old_2 < motor_state_2)
00206             count_2 += 1;
00207         else if(motor_state_old_2 > motor_state_2)
00208             count_2 -= 1;
00209         if(motor_state_old_2 == 4 && motor_state_2 == 1)
00210             count_2 += 2;
00211         if(motor_state_old_2 == 1 && motor_state_2 == 4)
00212             count_2 -= 2;
00213     }
00214     motor_state_old_2 = motor_state_2;
00215     
00216     //////////////////////////////////
00217     
00218     //Forward
00219     //v2Count +1
00220     //Inverse
00221     //v2Count -1
00222 }
00223 //****************************************************************************** End of CN_interrupt
00224  
00225 //****************************************************************************** init_timer
00226 void init_timer()
00227 {
00228      timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz)
00229 }
00230 //****************************************************************************** End of init_timer
00231  
00232 //****************************************************************************** init_PWM
00233 void init_PWM()
00234 {
00235     pwm1.period_us(50);
00236     pwm1.write(0.5);
00237     TIM1->CCER |= 0x4;
00238     
00239     pwm2.period_us(50);
00240     pwm2.write(0.5);
00241     TIM1->CCER |= 0x40;
00242     
00243     servo.period_ms(Servo_Period);
00244     servo.write(servo_duty);
00245 }
00246 //****************************************************************************** End of init_PWM
00247  
00248 //****************************************************************************** init_CN
00249 void init_CN()
00250 {
00251     // Motor1
00252     HallA_1.rise(&CN_interrupt);
00253     HallA_1.fall(&CN_interrupt);
00254     HallB_1.rise(&CN_interrupt);
00255     HallB_1.fall(&CN_interrupt);
00256     
00257     HallA_state_1 = HallA_1.read();
00258     HallB_state_1 = HallB_1.read();
00259     
00260     // Motor2
00261     HallA_2.rise(&CN_interrupt);
00262     HallA_2.fall(&CN_interrupt);
00263     HallB_2.rise(&CN_interrupt);
00264     HallB_2.fall(&CN_interrupt);
00265     
00266     HallA_state_2 = HallA_2.read();
00267     HallB_state_2 = HallB_2.read();
00268 }
00269 //****************************************************************************** End of init_CN