File content as of revision 0:cb8ba5eb9f7e:

#include "mbed.h"
#include "QEI.h"
#include "PID.h"


Serial bluetooth(D10, D2); //uart for bluretooth
Serial pc(D1, D0);         //uart for pc

QEI wheel_L (A1, A2, NC, 1560, 10, 0.01, QEI::X4_ENCODING); //390*4 = 1560 //chang array 50 to 10
QEI wheel_R (D13, D12, NC, 1560, 10, 0.01, QEI::X4_ENCODING);//390*4 = 1560 //chang array 50 to 10
PID speed_L_PID(0.1, 0.1, 0.0, 0.01);//0.00001, 0.00001, 0,,,//0.08, 0.08, 0.01, 0.01, 0.02
PID speed_R_PID(0.07, 0.07, 0.0, 0.01);//(0.1, 0.000005, 0, 0.01 //0.22, 0.11, 0.005, 0.01

PwmOut motor_L_pwm(D7);
PwmOut motor_L_pwmn(D11);
PwmOut motor_R_pwm(D8);
PwmOut motor_R_pwmn(A3);

Ticker timer1;
void timer1_interrupt(void);
void init_PWM(void);
void init_TIMER(void);
void init_sensor(void);
void real_sensor_value(void);
void sensor_fusion(void);
float lpf(float input, float output_old, float frequency);

float motor_L_AngularSpeed;
float motor_R_AngularSpeed;
float sample_speed = 0.0;
float pwm1_duty = 0.5f;
float pwm2_duty = 0.5f;

char Idle_Mode = 'a'; 
char robot_Mode = 'a';
char speed_2 = 'b';
char speed_3 = 'c';
char speed_4 = 'd';
char speed_5 = 'e';
char speed_6 = 'f';
char speed_7 = 'g';
char speed_8 = 'h';

int main() {
    
    pc.baud(115200);
    bluetooth.baud(115200);
    init_PWM();
    init_TIMER();
    
    while(1) 
    {
       ;
    }
}

void timer1_interrupt(void)
{      
    wheel_L.Calculate();
    wheel_R.Calculate();
    
    if(bluetooth.readable())
        {
            robot_Mode = bluetooth.getc();
        }
        
        if(robot_Mode == Idle_Mode)
        {
            //bluetooth.printf("Idle_Mode\n");             
            pwm1_duty = 0.5f;
            pwm2_duty = 0.5f; 
    
        }
        else if(robot_Mode == speed_2)
        {
            //bluetooth.printf("speed_2\n");
             
            sample_speed = 2.0;            

            speed_L_PID.Compute(-sample_speed, wheel_L.getAngularSpeed());
            speed_R_PID.Compute(sample_speed, wheel_R.getAngularSpeed()); 
            
            pwm1_duty = 0.5f + speed_L_PID.output;
            pwm2_duty = 0.5f + speed_R_PID.output;    

        }
        else if(robot_Mode == speed_3)
        {
            //bluetooth.printf("speed_3\n"); 
            
            sample_speed = 3.0;             
            
            speed_L_PID.Compute(sample_speed, wheel_L.getAngularSpeed());
            speed_R_PID.Compute(-sample_speed, wheel_R.getAngularSpeed()); 
            
            pwm1_duty = 0.5f + speed_L_PID.output;
            pwm2_duty = 0.5f + speed_R_PID.output;
                           
        }
        else if(robot_Mode == speed_4)
        {
            //bluetooth.printf("speed_4\n");
            
            sample_speed = 4.0;              
            
            speed_L_PID.Compute(-sample_speed, wheel_L.getAngularSpeed());
            speed_R_PID.Compute(sample_speed, wheel_R.getAngularSpeed()); 
            
            pwm1_duty = 0.5f + speed_L_PID.output;
            pwm2_duty = 0.5f + speed_R_PID.output;      
        }
        else if(robot_Mode == speed_5)
        {
            //bluetooth.printf("speed_5\n");
            
            sample_speed = 5.0;               
            
            speed_L_PID.Compute(sample_speed, wheel_L.getAngularSpeed());
            speed_R_PID.Compute(-sample_speed, wheel_R.getAngularSpeed()); 
            
            pwm1_duty = 0.5f + speed_L_PID.output;
            pwm2_duty = 0.5f + speed_R_PID.output;   
        }
        else if(robot_Mode == speed_6)
        {
            //bluetooth.printf("speed_6\n");
            
            sample_speed = 6.0;            
               
            speed_L_PID.Compute(-sample_speed, wheel_L.getAngularSpeed());
            speed_R_PID.Compute(sample_speed, wheel_R.getAngularSpeed()); 
            
            pwm1_duty = 0.5f + speed_L_PID.output;
            pwm2_duty = 0.5f + speed_R_PID.output;
                   
        }
        else if(robot_Mode == speed_7)
        {
            //bluetooth.printf("speed_7\n");
            
            sample_speed = 7.0;            
              
            speed_L_PID.Compute(sample_speed, wheel_L.getAngularSpeed());
            speed_R_PID.Compute(-sample_speed, wheel_R.getAngularSpeed()); 
            
            pwm1_duty = 0.5f + speed_L_PID.output;
            pwm2_duty = 0.5f + speed_R_PID.output;
     
        }
        else if(robot_Mode == speed_8)
        {
            //bluetooth.printf("speed_8\n");
            
            sample_speed = 8.0;            
                           
            speed_L_PID.Compute(-sample_speed, wheel_L.getAngularSpeed());
            speed_R_PID.Compute(sample_speed, wheel_R.getAngularSpeed()); 
                
            pwm1_duty = 0.5f + speed_L_PID.output;
            pwm2_duty = 0.5f + speed_R_PID.output; 
     
        }
        else
        {        
            pwm1_duty = 0.5f;
            pwm2_duty = 0.5f;     
        } 
          
        motor_L_pwm.write(pwm1_duty); //>0.5 forward(-) ; <0.5 backward(+)    
        motor_R_pwm.write(pwm2_duty); // <0.5 forward(-) ; >0.5 backward(+)
        
        TIM1->CCER |= 4; //enable ch1 complimentary output
        TIM1->CCER |= 64; //enable ch2 complimentary output     

 
        pc.printf("motor_L_AngularSpeed: %f motor_R_AngularSpeed: %f \r\n",wheel_L.getAngularSpeed(), wheel_R.getAngularSpeed());
}

void init_TIMER(void)
{
    timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
}         
   
void init_PWM(void)
{
    motor_L_pwm.period_us(50);
    motor_L_pwm.write(0.5f);
    
    motor_R_pwm.period_us(50);
    motor_R_pwm.write(0.5f);
      
    TIM1->CCER |= 4;
    TIM1->CCER |= 64;
    
    speed_L_PID.SetOutputLimits(0.5, -0.5);
    speed_R_PID.SetOutputLimits(0.5, -0.5);
}