Diff: communication/communication.cpp

Revision:

0:489498e8dae5

diff -r 000000000000 -r 489498e8dae5 communication/communication.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/communication/communication.cpp	Fri Aug 25 21:10:23 2017 +0000
@@ -0,0 +1,315 @@
+//////////////////////////////////////////////////////////////////////////////////
+// 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;
+        }
+    }
+}
\ No newline at end of file