File content as of revision 1:ce626b794a6c:

//////////////////////////////////////////////////////////////////////////////////
// Company: edinburgh of university
// Engineer: ZEjun DU
// 
// Create Date: 2017/08/20 13:06:52
// Design Name: Inverted Pendulum Balancer
// Module Name: commnication with GUI
// Tool Versions: “Keil 5” or “Mbed Complie Online”
// Description: this funcition is to commucation with GUI through Bluetooth HC05
//              the system can invoke this function to send the packet and decode the result
//              Pin 28 --------> (TX) RX pin of the Bluetooth module 
//              Pin 27 --------> (RX) TX pin of the Bluetooth  module
//              GND ----------->  GND of the Bluetooth module
//              VOUT (3.3 V) -->  VCC of the Bluetooth module
// 
//////////////////////////////////////////////////////////////////////////////////

#include "communication.h"
#include "get_angle.h"
#include "controller.h"
#include "stabilizer.h"
#include "distance.h"

Serial bluetooth(p28,p27);    // p28: TX, p27: RX (LPC1768)

DigitalOut led1(LED1);//LED init
DigitalOut led2(LED2);//LED init
DigitalOut led3(LED3);//LED init
DigitalOut led4(LED4);//LED init

int j = 500;//the defualt PWM(50% PWM)
int i = 0; //the Bytes' numbers
float AY, EP, ER;//redundant variables

uint8_t usart_rx_buf[64];//this variable is to load the packet from GUI

uint8_t Tx_Buf[24] = //this is the buff which should be sent to GUI 
{
    'A','T',
    '0','0','0','0',
    '0','0','0','0',
    '0','0','0','0',
    '0','0','0','0',
    '0','0','0','0',
    '0','0'
};


////////////////////////////////////////////////////////////
//this funcition is to load the data (distance and angle) and send the whole packet
////////////////////////////////////////////////////////////
void Send_Messge(void)
{
    int X;//the number of current Byte

    Tx_Buf[2] = (int32_t)angle>>24;//send 8 bits each time
    Tx_Buf[3] = (int32_t)angle>>16;//angle
    Tx_Buf[4] = (int32_t)angle>>8;
    Tx_Buf[5] = (int32_t)angle;
    
    Tx_Buf[6] = (int32_t)distance>>24;//send 8 bits each time
    Tx_Buf[7] = (int32_t)distance>>16;//distance
    Tx_Buf[8] = (int32_t)distance>>8;
    Tx_Buf[9] = (int32_t)distance;
    
    Tx_Buf[10] = (int32_t)AY>>24;//send 8 bits each time
    Tx_Buf[11] = (int32_t)AY>>16;
    Tx_Buf[12] = (int32_t)AY>>8;
    Tx_Buf[13] = (int32_t)AY;
    
    Tx_Buf[14] = (int32_t)EP>>24;//send 8 bits each time
    Tx_Buf[15] = (int32_t)EP>>16;
    Tx_Buf[16] = (int32_t)EP>>8;
    Tx_Buf[17] = (int32_t)EP;
    
    Tx_Buf[18] = (int32_t)ER>>24;//send 8 bits each time
    Tx_Buf[19] = (int32_t)ER>>16;
    Tx_Buf[20] = (int32_t)ER>>8;
    Tx_Buf[21] = (int32_t)ER;
    
    Tx_Buf[22] = 0;
    Tx_Buf[23] = 0;
//    bluetooth.printf("this sentence is used to test");
    
    for(X=0; X<24; X++)
    { 
        bluetooth.printf("%c", Tx_Buf[X]);//send the packet         
    }    
}


////////////////////////////////////////////////////////////
//this funcition is to deocode the data from GUI
////////////////////////////////////////////////////////////
void commander(void)
{
    usart_rx_buf[i] = bluetooth.getc();
    i++;
    if(i==6){
        i=0;  
        if( (usart_rx_buf[0]=='G') && (usart_rx_buf[1]=='O'))//"GO"
        {
            PID_ctrl = 1;//enable PID mode
            Velocity_ctrl = 1;//enable velocity PID
                       
            led1 = 1;
            led2 = 0;
            led3 = 0;
            led4 = 0;
        }//unlock the inverted pendulum, its means that we start the pid control 
        
        if( (usart_rx_buf[0]=='S') && (usart_rx_buf[1]=='T'))//"STOP"
        {
            PID_ctrl = 0;//disable PID mode
            Velocity_ctrl = 0;//disable velocity PID
            
            led1 = 0;            
            led2 = 1;
            led3 = 0;
            led4 = 0;
            MOTOR_Update(0);
        }//shut down all controlling 
        
        if( (usart_rx_buf[0]=='A') && (usart_rx_buf[1]=='U'))//keyboard control
        {
            if(usart_rx_buf[2]=='1'){//go forward manually
                Motor_PWM.Stop();//to avoid turnning on all switches of L298n. turnning on all switches will damage the L298n
                wait_us(50);
                Motor_PWM.Fwd();
                Motor_PWM.Speed(j);
                led1 = 0;            
                led2 = 0;
                led3 = 1;
                led4 = 0;
                
                }
            else if(usart_rx_buf[2]=='2'){//go reverse manually
                Motor_PWM.Stop();
                wait_us(50);
                Motor_PWM.Rev();
                Motor_PWM.Speed(j);
                led1 = 0;            
                led2 = 0;
                led3 = 0;
                led4 = 1;
                }  
            else if(usart_rx_buf[2]=='3'){//change the force to motor manually
                j=j+100;
                if(j>=1000)
                j=1000;
                Motor_PWM.Speed(j);
                led1 = 0;            
                led2 = 0;
                led3 = 1;
                led4 = 0;
                }
            else if(usart_rx_buf[2]=='4'){//change the force to motor manually
                j=j-100;
                if(j<=0)
                j=0;
                Motor_PWM.Speed(j);
                led1 = 0;            
                led2 = 0;
                led3 = 0;
                led4 = 1;
                }
            else if(usart_rx_buf[2]=='5'){
                PID_ctrl = 0;
            
                led1 = 1;            
                led2 = 1;
                led3 = 0;
                led4 = 0;
                MOTOR_Update(0);
                }
                
            else if(usart_rx_buf[2]=='6'){//set the original of angle and distance
                angle_offset = angle_voltage.read();
                usensor.start();
                home = usensor.get_dist_cm();
            
                led1 = 1;            
                led2 = 1;
                led3 = 1;
                led4 = 1;
                
                }
            else if(usart_rx_buf[2]=='7'){//set the original of distance
                usensor.start();
                home = usensor.get_dist_cm();
                Velocity_ctrl = 1;
            
                led1 = 1;            
                led2 = 0;
                led3 = 1;
                led4 = 0;
                
                }
        }// change the direction of car by keyboard
        

        

        //change the volecity of car   
        if(usart_rx_buf[0] == 'P' && usart_rx_buf[1] == 'W')
        {
            M_Thr = (usart_rx_buf[2] << 8) | usart_rx_buf[3];
            MOTOR_Update(M_Thr);
        }
        
        //Launch Control 
        if(usart_rx_buf[0] == 'L' && usart_rx_buf[1] == 'C')
        {
            PID_ctrl = 0;
            Velocity_ctrl = 0;
            if(angle <=0)
            {
            Motor_PWM.Rev();
            Motor_PWM.Speed(900);
            wait(0.7);
            Motor_PWM.Fwd();
            Motor_PWM.Speed(700);
            wait(0.3);
            }
            else
            {
            Motor_PWM.Fwd();
            Motor_PWM.Speed(900);
            wait(0.7);
            Motor_PWM.Rev();
            Motor_PWM.Speed(700);
            wait(0.3);
            }
            PID_ctrl = 1;
            Velocity_ctrl = 1;
            
        }
        
        
        
        //this is function for distance
        //to set the 'home' point
        if( (usart_rx_buf[0]=='H') && (usart_rx_buf[1]=='O'))
        {
            usensor.start();
            home = usensor.get_dist_cm();
        }        
        
        if(usart_rx_buf[0] == 'S' && usart_rx_buf[1] == 'H')
        {
            Velocity_ctrl = 0;
            M_Alt = 0;
        } 
        
        if(usart_rx_buf[0] == 'H' && usart_rx_buf[1] == 'P')
        {
            pidVelocity.kp = -(usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
            
            pidVelocity.kp /= 1000;
        }
        if(usart_rx_buf[0] == 'H' && usart_rx_buf[1] == 'I')
        {
            pidVelocity.ki = -(usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
            
            pidVelocity.ki /= 1000;
        }
        if(usart_rx_buf[0] == 'H' && usart_rx_buf[1] == 'D')
        {
            pidVelocity.kd = -(usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
            
            pidVelocity.kd /= 1000;
        }
                         
        //change the distance
        if(usart_rx_buf[0] == 'E' && usart_rx_buf[1] == 'H')
        {
            pidVelocity.desired  = (usart_rx_buf[2] << 8) | usart_rx_buf[3];
            if(usart_rx_buf[4]=='-')           
            pidVelocity.desired  = -(pidVelocity.desired /1000);
            if(usart_rx_buf[4]=='+')           
            pidVelocity.desired  = pidVelocity.desired /1000;
        }    
                
        //the following part is to set PID of angle                 
        if(usart_rx_buf[0] == 'S' && usart_rx_buf[1] == 'P')
        {
            pidAngle.kp = (usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
    
            pidAngle.kp /= 1000;   
        }
                                    
        if(usart_rx_buf[0] == 'S' && usart_rx_buf[1] == 'I')
        {
            pidAngle.ki = (usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
            
            pidAngle.ki /= 1000;
        }
                                    
        if(usart_rx_buf[0] == 'S' && usart_rx_buf[1] == 'D')
        {
            pidAngle.kd = ( usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
                        
            pidAngle.kd /= 1000;
        }
        
        if(usart_rx_buf[0] == 'E' && usart_rx_buf[1] == 'P')
        {
            pidAngle.desired =( usart_rx_buf[2]<<8)|(usart_rx_buf[3]);
            if(usart_rx_buf[4]=='-')                           
                pidAngle.desired = -(pidAngle.desired /1000);
            if(usart_rx_buf[4]=='+')                            
                pidAngle.desired /= 1000;
        }
    }
}