Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
Diff: main.cpp
- Revision:
- 2:0f80c8a1c236
- Parent:
- 1:1da89c13dfa1
- Child:
- 3:d1197b5ea68a
diff -r 1da89c13dfa1 -r 0f80c8a1c236 main.cpp --- a/main.cpp Sat Oct 11 03:53:27 2014 +0000 +++ b/main.cpp Sun Oct 12 13:33:19 2014 +0000 @@ -7,33 +7,76 @@ #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]; -int bit_size=20; -int thetha1=300; -int thetha=0; -int stall=0; -int bump=1; -int UL_rR = 0; -int UL_R = 0; -int UL_F = 0; -int UL_L = 0; -int UL_rL = 0; -int UL_B = 0; -int dx1=100; -int dy1=200; - -int linear_velocity_value ; -int linear_velocity_direction; -int rotational_velocity_value ; -int rotational_velocity_direction; -int Lspeed=1; -int Rspeed=1; +void bt_shell_thr(void const *args) +{ + while(true) { + bt_shell_run(); + Thread::wait(50); + } +} int main() { initRobot(); - //motor_control((100),(-100)); while(buffer[3] != 'O') { while(buffer[3] != 'E') { if(bt.readable()) { @@ -41,7 +84,7 @@ if(buffer[3] == 'E') { bt.printf(">>1B"); } else if(buffer[3] == '?') { - bt.printf("K"); + bt.printf("eBot#2"); } } } @@ -49,106 +92,13 @@ bt.gets(buffer, 5); } } - //imperial_march(); Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL); Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL); - buzzer=0; - //motor_control(lMotor*m_speed,rMotor*m_speed*0.8); - //motor_control(lMotor*m_speed,0); + 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) { - //bt.lock(); - //bt.printf("LDR1_%lf;LDR2_%lf;Pulses is: %i Revolutions is: %i Pulses is: %i ultrasonic: , \r\n",ldrread1(),ldrread2(), left.getPulses(), left.getRevolutions(),right.getPulses());//, uL.read()*5*102.54*2.54); - //bt.printf("s;LDR1_%lf;LDR2_%lf;Pulses is: %i ; %i:s\n\r",ldrread1(),ldrread2(),wheel.getPulses(), wheel.getRevolutions()); - // motor_control(lMotor*m_speed,rMotor*m_speed); - //bt.printf("a/g:\t %d:\t %d:\t %d:\t %d:\t %d:\t %d:\t\r\n",ax ,ay,az,gx,gy,gz); - //bt.unlock(); - if(bt.readable()) { - bt.gets(buffer, 5); - if(buffer[3] == 'S') { - SRX=1; - wait_ms(2); - UL_rR=sensorvalue(urR); - SRX=0; - wait_ms(2); - - SRX=1; - wait_ms(2); - UL_R=sensorvalue(uR); - SRX = 0; - wait_ms(2); - - SRX=1; - wait_ms(2); - UL_F=sensorvalue(uF); - SRX=0; - wait_ms(2); - - SRX=1; - wait_ms(2); - UL_L=sensorvalue(uL); - SRX=0; - wait_ms(2); - - SRX=1; - wait_ms(2); - UL_rL=sensorvalue(urL); - SRX=0; - wait_ms(2); - - SRX=1; - wait_ms(2); - UL_B=sensorvalue(uB); - SRX=0; - - bt.lock(); - stdio_mutex.lock(); - heading=heading*11.375; - bt.printf(">>D%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", - dx/256,dx%256,dy/256,dy%256,\ - (heading)/256,(heading)%256,\ - stall, bump,\ - UL_rR/256,UL_rR%256,\ - UL_R/256,UL_R%256, \ - UL_F/256,UL_F%256,\ - UL_L/256,UL_L%256,\ - UL_rL/256,UL_rL%256,\ - UL_B/256,UL_B%256); - dx=0; - dy=0; - stdio_mutex.unlock(); - bt.unlock(); - } else if (buffer[3] == 'R') { - Thread::wait(20); - if(bt.readable()) { - bt.gets(buffer, 10); - linear_velocity_value = buffer[4]<<8|buffer[3]; - linear_velocity_direction= buffer[5]; - rotational_velocity_value = buffer[7]<<8|buffer[6]; - rotational_velocity_direction= buffer[8]; - if( linear_velocity_direction==0x01) { - Lspeed=lMotor; - Rspeed=rMotor; - } else if( linear_velocity_direction==0x10) { - Lspeed=-lMotor; - Rspeed=-rMotor; - } - if( rotational_velocity_direction==0x01 && linear_velocity_value !=0) { - - - motor_control((Lspeed*linear_velocity_value/35)-10,(Rspeed*linear_velocity_value/35) + 10); - - } else if( rotational_velocity_direction==0x10 && linear_velocity_value !=0) { - motor_control((Lspeed*linear_velocity_value/35)+10,(Rspeed*linear_velocity_value/30) -10); - } - else - motor_control((Lspeed*linear_velocity_value/30),(Rspeed*linear_velocity_value/30)); - - } - } - //memset(buffer, 0, sizeof buffer); - } - if(ldrread1()>0.6) { } if(ldrread2()>0.6) { @@ -156,6 +106,7 @@ } else { Led_off(); } - Thread::wait(20); + ultrasonic_run(); + Thread::yield(); } -} \ No newline at end of file +}