main.cpp

00001 // FORMAT_CODE_START
00002 #include "QEI.h"
00003 #include "mbed.h"
00004 #include "robot.h"
00005 #include "rtos.h"
00006 #include "MPU6050.h"
00007 #include "I2Cdev.h"
00008 #include "tone.h"
00009 #include "ultrasonic.h"
00010 #include "bt_shell.h"
00011 #include "bt_shell_f.h"
00012 int Current_Right_pulse=0;
00013 int Current_Left_pulse=0;
00014 int Linput=0;
00015 int Rinput=0;
00016 int Change_Right_pulse=0;
00017 int Change_Left_pulse=0;
00018 int Previous_Left_pulse=0;
00019 int Previos_Right_pulse=0;
00020 int time_int = 50;
00021 int time_factor=1000/time_int ;
00022 int Error_Right=0;
00023 int Error_Left=0;
00024 float L_Kp=0.1;
00025 float R_Kp=0.1;
00026 int Last_Error_Left=0;
00027 int Last_Error_Right=0;
00028 int previous_Linput= 0 ;
00029 int previous_Rinput= 0 ;
00030 RtosTimer *Motor_controller_Timer;
00031 
00032 void Motorcontroller(void const *args)
00033 {
00034 
00035     Current_Right_pulse= right.getPulses()/10;
00036     Current_Left_pulse=left.getPulses()*(-1)/9.85;
00037 
00038     Change_Right_pulse=Current_Right_pulse- Previos_Right_pulse;
00039     Change_Left_pulse=Current_Left_pulse- Previous_Left_pulse;
00040 
00041     Previous_Left_pulse=Current_Left_pulse;
00042     Previos_Right_pulse=Current_Right_pulse;
00043 
00044     Error_Right=(Rmotor_speed- (Change_Right_pulse*time_factor) );
00045     Error_Left=(Lmotor_speed- (Change_Left_pulse*time_factor));
00046     Linput=(Linput+Error_Left*L_Kp);
00047     Rinput=(Rinput+Error_Right*R_Kp);
00048     if(Linput>100)
00049         Linput=100;
00050     else if (Linput<-100)
00051         Linput= -100;
00052     if(Rinput>100)
00053         Rinput=100;
00054     else if(Rinput<-100)
00055         Rinput= -100;
00056     if(Error_Right==0) {
00057         Last_Error_Right++;
00058     } else {
00059         Last_Error_Right=0;
00060     }
00061     if(Error_Left==0) {
00062         Last_Error_Left++;
00063     } else {
00064         Last_Error_Left=0;
00065     }
00066     if(Last_Error_Right==100) {
00067         Rinput=0;
00068     }
00069     if(Last_Error_Left==100) {
00070         Linput=0;
00071     }
00072     *motors.Left = Linput;          // HARSH change to -Linput for black and Linput for blue
00073     *motors.Right = -Rinput;        //// HARSH change to Rinput for black and -Rinput for blue
00074 }
00075 char c;
00076 char buffer[10];
00077 void bt_shell_thr(void const *args)
00078 {
00079     while(true) {
00080         if(Selected_robot=='A') {
00081             bt_shell_run();
00082         } else if(Selected_robot=='F') {
00083             bt_shell_f_run();
00084         }
00085         Thread::wait(50);
00086     }
00087 }
00088 
00089 int main()
00090 {
00091     initRobot();
00092     while(buffer[3] != 'O') {
00093         while(buffer[3] != 'E') {
00094             if(bt.readable()) {
00095                 bt.gets(buffer, 5);
00096                 if(buffer[3] == 'E') {
00097                     bt.printf(">>1B");
00098                 } else if(buffer[3] == '?') {
00099                     bt.printf("eBot#2");
00100                 }
00101             }
00102         }
00103         if(bt.readable()) {
00104             bt.gets(buffer, 5);
00105         }
00106     }
00107     bt_connected=true;
00108     bt.gets(buffer, 2);
00109     if(buffer[0]=='A') {
00110         Selected_robot='A';
00111         //imperial_march();
00112 
00113     } else if(buffer[0]=='F') {
00114         Selected_robot='F';
00115         Led_on();
00116     }
00117     Thread bt_shell_th(bt_shell_thr, NULL, osPriorityNormal, 2048, NULL);
00118     Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL);
00119     Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL);
00120     Motor_controller_Timer = new RtosTimer(&Motorcontroller, osTimerPeriodic, NULL);
00121     Motor_controller_Timer->start(time_int);
00122     while(1) {
00123         if(Selected_robot=='A') {
00124             if(ldrread2()>0.6) {
00125                 Led_on();
00126             } else {
00127                 Led_off();
00128             }
00129         }
00130         ultrasonic_run();
00131         Thread::yield();
00132     }
00133 }