Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
main.cpp
- Committer:
- Throwbot
- Date:
- 2014-10-12
- Revision:
- 2:0f80c8a1c236
- Parent:
- 1:1da89c13dfa1
- Child:
- 3:d1197b5ea68a
File content as of revision 2:0f80c8a1c236:
// 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" int Rmotor_input=0; int Lmotor_input=0; int Current_Right_pulse=0; int Current_Left_pulse=0; int Error_Right=0; int Error_Left=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 Last_Error_Right=0; int Last_Error_Left=0; int Rdistance_factor=0; int Ldistance_factor=0; int time_int = 50; int time_factor=1000/time_int ; int L_Kp=0; int R_Kp=0; RtosTimer *Motor_controller_Timer; void Motorcontroller(void const *args) { Current_Right_pulse= right.getPulses(); Current_Left_pulse=left.getPulses(); 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_input- (Change_Right_pulse*time_factor*Rdistance_factor) ); Error_Left=(Lmotor_input- (Change_Left_pulse*time_factor*Ldistance_factor)); Last_Error_Right=Error_Right; Last_Error_Left=Error_Left; Linput=(Linput+Error_Left*L_Kp); Rinput=(Rinput+Error_Right*R_Kp); if(Linput>100) Linput=100; else if (Linput<-100) Linput= -100; else if (-21<Linput && Linput<21) Linput= 0; if(Rinput>100) Rinput=100; else if(Rinput<-100) Rinput= -100; else if (-21<Rinput && Rinput<21) Rinput= 0; } char c; char buffer[10]; void bt_shell_thr(void const *args) { while(true) { bt_shell_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); } } //imperial_march(); Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL); Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL); Thread bt_shell_th(bt_shell_thr, NULL, osPriorityNormal, 2048, NULL); Motor_controller_Timer = new RtosTimer(&Motorcontroller, osTimerPeriodic, NULL); //Motor_controller_Timer->start(time_int); while(1) { if(ldrread1()>0.6) { } if(ldrread2()>0.6) { Led_on(); } else { Led_off(); } ultrasonic_run(); Thread::yield(); } }