Revision 1:1523bf60b9a9, committed 2018-04-19

Comitter:

MechanicalThomas

Date:

Thu Apr 19 14:57:05 2018 +0000

Parent:

0:fd24f7ca9688

Commit message:

000;

Changed in this revision

diff -r fd24f7ca9688 -r 1523bf60b9a9 keyboard.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/keyboard.cpp	Thu Apr 19 14:57:05 2018 +0000
@@ -0,0 +1,323 @@
+/* LAB DCMotor */
+#include "mbed.h"
+#include <ros.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Twist>
+ 
+//****************************************************************************** Define
+//The number will be compiled as type "double" in default
+//Add a "f" after the number can make it compiled as type "float"
+#define Ts 0.01f    //period of timer1 (s)
+ 
+//****************************************************************************** End of Define
+ 
+//****************************************************************************** I/O
+//PWM
+//Dc motor
+PwmOut pwm1(D7);
+PwmOut pwm1n(D11);
+PwmOut pwm2(D8);
+PwmOut pwm2n(A3);
+ 
+//Motor1 sensor
+InterruptIn HallA_1(A1);
+InterruptIn HallB_1(A2);
+//Motor2 sensor
+InterruptIn HallA_2(D13);
+InterruptIn HallB_2(D12);
+ 
+//LED
+DigitalOut led1(A4);
+DigitalOut led2(A5);
+ 
+//Timer Setting
+Ticker timer;
+//****************************************************************************** End of I/O
+ 
+//****************************************************************************** Functions
+void init_timer(void);
+void init_CN(void);
+void init_PWM(void);
+void timer_interrupt(void);
+void CN_interrupt(void);
+//****************************************************************************** End of Functions
+ 
+//****************************************************************************** Variables
+// Servo
+float servo_duty = 0.066; // 0.025~0.113(-90~+90) 0.069->0 degree
+
+// motor 1
+int8_t HallA_state_1 = 0;
+int8_t HallB_state_1 = 0;
+int8_t motor_state_1 = 0;
+int8_t motor_state_old_1 = 0;
+int count_1 = 0;
+float speed_1 = 0.0f;
+float v_ref_1 = 80.0f;
+float v_err_1 = 0.0f;     //   v_err_old_1 = v_err_1 ;  v_err_1 = v_ref_1 - speed_1 ;
+float v_ierr_1 = 0.0f;   //integral error : v_ierr_1 = v_err_old_1 + v_err_1;
+float ctrl_output_1 = 0.0f;
+float pwm1_duty = 0.0f;
+
+float Kp_1 = 50;  //need to be tested
+float Ki_1 = 100;  //need to be tested
+float ctrl_output_old_1 = 0.0f;
+
+
+//motor 2
+int8_t HallA_state_2 = 0;
+int8_t HallB_state_2 = 0;
+int8_t motor_state_2 = 0;
+int8_t motor_state_old_2 = 0;
+int count_2 = 0;
+float speed_2 = 0.0f;
+float v_ref_2 = 150.0f;
+float v_err_2 = 0.0f;
+float v_ierr_2 = 0.0f;
+float ctrl_output_2 = 0.0f;
+float pwm2_duty = 0.0f;
+
+
+float Kp_2 = 50;
+float Ki_2 = 100;
+float ctrl_output_old_2 = 0.0f;
+
+
+//****************************************************************************** End of Variables
+ 
+ 
+//****************************************************************************** Main
+int main()
+{
+    init_PWM();
+    init_timer();
+    init_CN();
+    while(1)
+    {
+    }
+}
+//****************************************************************************** End of Main
+ 
+//****************************************************************************** timer_interrupt
+void timer_interrupt()
+{   
+    // Motor1
+    speed_1 = (float)count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm of output wheel (period=0.01 sec, each period has 12 segments, reduction ratio 29)
+    count_1 = 0;
+    // Code for PI controller //
+    
+      
+    v_err_1 = v_ref_1 - speed_1 ;
+    
+    ctrl_output_1 = (ctrl_output_old_1) + (Kp_1+Ki_1*Ts*0.5f)*(v_err_1) + (-Kp_1+Ki_1)*0.5f*(v_ierr_1);
+    
+    v_ierr_1 =  v_err_1 ;
+    ctrl_output_old_1 = ctrl_output_1;
+
+    
+    ///////////////////////////
+    
+    if(ctrl_output_1 >= 0.5f)ctrl_output_1 = 0.5f;
+    else if(ctrl_output_1 <= -0.5f)ctrl_output_1 = -0.5f;
+    pwm1_duty = ctrl_output_1 + 0.5f;
+    pwm1.write(pwm1_duty);
+    TIM1->CCER |= 0x4;
+    
+    
+    
+    // Motor2
+    speed_2 = (float)count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
+    count_2 = 0;
+    // Code for PI controller //
+      
+    v_err_2 = v_ref_2 - speed_2 ;
+    
+    ctrl_output_2 = (ctrl_output_old_2) + (Kp_2+Ki_2*Ts*0.5f)*(v_err_2) + (-Kp_2+Ki_2)*0.5f*(v_ierr_2);
+    
+    v_ierr_2 =  v_err_2 ;
+    ctrl_output_old_2 = ctrl_output_2;
+    
+    ///////////////////////////    
+    
+    if(ctrl_output_2 >= 0.5f)ctrl_output_2 = 0.5f;
+    else if(ctrl_output_2 <= -0.5f)ctrl_output_2 = -0.5f;
+    pwm2_duty = ctrl_output_2 + 0.5f;
+    pwm2.write(pwm2_duty);
+    TIM1->CCER |= 0x40;
+    
+}
+//****************************************************************************** End of timer_interrupt
+ 
+//****************************************************************************** CN_interrupt
+void CN_interrupt()
+{
+    // Motor1
+    // Read the current status of hall sensor
+    HallA_state_1 = HallA_1.read();
+    HallB_state_1 = HallB_1.read();
+     
+   ///code for state determination///
+    if(HallA_state_1==0)
+    {
+        if(HallB_state_1==0)
+        {
+        motor_state_1 =1;
+        }
+        else
+        {
+         motor_state_1 =2;   
+            }
+        }      
+    else
+    {
+        if(HallB_state_1==0)
+        {
+            motor_state_1 = 3;
+            }
+            else
+            {
+                motor_state_1 = 4;
+                }
+        }
+         
+    
+    //////////////////////////////////
+    switch(motor_state_1)
+    {
+        case 1:
+            if(motor_state_old_1 == 4) 
+            count_1--;           
+            else if(motor_state_old_1 == 2) 
+            count_1++;  
+            break; 
+        case 2:
+            if(motor_state_old_1 == 1) 
+            count_1--;                 
+            else if(motor_state_old_1 == 3) 
+            count_1++;  
+            break; 
+        case 3:
+            if(motor_state_old_1== 2)  
+            count_1--;                 
+            else if(motor_state_old_1 == 4) 
+            count_1++;  
+            break; 
+        case 4:
+            if(motor_state_old_1 == 3) 
+            count_1--;  
+            else if(motor_state_old_1 == 1) 
+            count_1++;  
+            break; 
+    }
+    motor_state_old_1 = motor_state_1;
+    //Forward
+    //v1Count +1
+    //Inverse
+    //v1Count -1
+        
+    // Motor2
+    // Read the current status of hall sensor
+    HallA_state_2 = HallA_2.read();
+    HallB_state_2 = HallB_2.read();
+     
+    ///code for state determination///
+   if(HallA_state_2==0)
+    {
+        if(HallB_state_2==0)
+        {
+        motor_state_2 =1;
+        }
+        else
+        {
+         motor_state_2 =2;   
+            }
+        }        
+    else
+    {
+        if(HallB_state_2==0)
+        {
+            motor_state_2 = 3;
+            }
+            else
+            {
+                motor_state_2 = 4;
+                }
+        }
+      
+    //////////////////////////////////
+        switch(motor_state_2)
+    {
+        case 1:
+            if(motor_state_old_2 == 4) 
+            count_2--;           
+            else if(motor_state_old_2 == 2) 
+            count_2++;  
+            break; 
+        case 2:
+            if(motor_state_old_2 == 1) 
+            count_1--;                 
+            else if(motor_state_old_2 == 3) 
+            count_2++;  
+            break; 
+        case 3:
+            if(motor_state_old_2== 2)  
+            count_2--;                 
+            else if(motor_state_old_2 == 4) 
+            count_2++;  
+            break; 
+        case 4:
+            if(motor_state_old_2 == 3) 
+            count_2--;  
+            else if(motor_state_old_2 == 1) 
+            count_2++;  
+            break; 
+    }
+    motor_state_old_2 = motor_state_2;
+    
+    //Forward
+    //v2Count +1
+    //Inverse
+    //v2Count -1
+}
+//****************************************************************************** End of CN_interrupt
+ 
+//****************************************************************************** init_timer
+void init_timer()
+{
+     timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz)
+}
+//****************************************************************************** End of init_timer
+ 
+//****************************************************************************** init_PWM
+void init_PWM()
+{
+    pwm1.period_us(50);
+    pwm1.write(0.5);
+    TIM1->CCER |= 0x4;
+    
+    pwm2.period_us(50);
+    pwm2.write(0.5);
+    TIM1->CCER |= 0x40;
+}
+//****************************************************************************** End of init_PWM
+ 
+//****************************************************************************** init_CN
+void init_CN()
+{
+    // Motor1
+    HallA_1.rise(&CN_interrupt);
+    HallA_1.fall(&CN_interrupt);
+    HallB_1.rise(&CN_interrupt);
+    HallB_1.fall(&CN_interrupt);
+    
+
+    
+    // Motor2
+    HallA_2.rise(&CN_interrupt);
+    HallA_2.fall(&CN_interrupt);
+    HallB_2.rise(&CN_interrupt);
+    HallB_2.fall(&CN_interrupt);
+    
+
+}
+//****************************************************************************** End of init_CN
\ No newline at end of file