Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
main.cpp
- Committer:
- Throwbot
- Date:
- 2014-10-14
- Revision:
- 4:81b0de07841f
- Parent:
- 3:d1197b5ea68a
- Child:
- 5:eb706d3e512c
File content as of revision 4:81b0de07841f:
// FORMAT_CODE_START #include "QEI.h" #include "mbed.h" #include "robot.h" #include "rtos.h" #include "MPU6050.h" #include "I2Cdev.h" #include "tone.h" #include "ultrasonic.h" #include "bt_shell.h" #include "bt_shell_f.h" int Current_Right_pulse=0; int Current_Left_pulse=0; int Linput=0; int Rinput=0; int Change_Right_pulse=0; int Change_Left_pulse=0; int Previous_Left_pulse=0; int Previos_Right_pulse=0; int time_int = 50; int time_factor=1000/time_int ; int Error_Right=0; int Error_Left=0; float L_Kp=0.1; float R_Kp=0.1; int Last_Error_Left=0; int Last_Error_Right=0; int previous_Linput= 0 ; int previous_Rinput= 0 ; RtosTimer *Motor_controller_Timer; void Motorcontroller(void const *args) { Current_Right_pulse= right.getPulses()/5; Current_Left_pulse=left.getPulses()*(-1)/5; Change_Right_pulse=Current_Right_pulse- Previos_Right_pulse; Change_Left_pulse=Current_Left_pulse- Previous_Left_pulse; Previous_Left_pulse=Current_Left_pulse; Previos_Right_pulse=Current_Right_pulse; Error_Right=(Rmotor_speed- (Change_Right_pulse*time_factor) ); Error_Left=(Lmotor_speed- (Change_Left_pulse*time_factor)); Linput=(Linput+Error_Left*L_Kp); Rinput=(Rinput+Error_Right*R_Kp); if(Linput>100) Linput=100; else if (Linput<-100) Linput= -100; if(Rinput>100) Rinput=100; else if(Rinput<-100) Rinput= -100; if(Error_Right==0) { Last_Error_Right++; } else { Last_Error_Right=0; } if(Error_Left==0) { Last_Error_Left++; } else { Last_Error_Left=0; } if(Last_Error_Right==100) { Rinput=0; } if(Last_Error_Left==100) { Linput=0; } *motors.Left = Linput; // HARSH change to -Linput for black and Linput for blue *motors.Right = -Rinput; //// HARSH change to Rinput for black and -Rinput for blue } char c; char buffer[10]; void bt_shell_thr(void const *args) { while(true) { if(Selected_robot=='A') { bt_shell_run(); } else if(Selected_robot=='F') { bt_shell_f_run(); } Thread::wait(50); } } int main() { initRobot(); while(buffer[3] != 'O') { while(buffer[3] != 'E') { if(bt.readable()) { bt.gets(buffer, 5); if(buffer[3] == 'E') { bt.printf(">>1B"); } else if(buffer[3] == '?') { bt.printf("eBot#2"); } } } if(bt.readable()) { bt.gets(buffer, 5); } } bt_connected=true; bt.gets(buffer, 2); if(buffer[0]=='A') { Selected_robot='A'; imperial_march(); } else if(buffer[0]=='F') { Selected_robot='F'; Led_on(); } Thread bt_shell_th(bt_shell_thr, NULL, osPriorityNormal, 2048, NULL); Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL); Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL); Motor_controller_Timer = new RtosTimer(&Motorcontroller, osTimerPeriodic, NULL); Motor_controller_Timer->start(time_int); while(1) { if(Selected_robot=='A') { if(ldrread2()>0.6) { Led_on(); } else { Led_off(); } } ultrasonic_run(); Thread::yield(); } }