Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
Diff: main.cpp
- Revision:
- 1:1da89c13dfa1
- Parent:
- 0:43331220df0d
- Child:
- 2:0f80c8a1c236
diff -r 43331220df0d -r 1da89c13dfa1 main.cpp --- a/main.cpp Sun Oct 05 15:27:32 2014 +0000 +++ b/main.cpp Sat Oct 11 03:53:27 2014 +0000 @@ -1,60 +1,161 @@ +// FORMAT_CODE_START #include "QEI.h" #include "mbed.h" #include "robot.h" #include "rtos.h" -#define ulrL PTD6 -#define ulR PTD5 -#define ulF PTD1 -#define ulrR PTC2 -#define ulB PTC1 -#define ulL PTC0 -AnalogIn uL(ulL); -AnalogIn uF(ulF); -AnalogIn uR(ulR); -AnalogIn urR(ulrR); -AnalogIn urL(ulrL); +#include "MPU6050.h" +#include "I2Cdev.h" +#include "tone.h" +#include "ultrasonic.h" char c; -int main() { - initRobot(); - Thread buzzer_th(pl_buzzer, NULL, osPriorityNormal, 2048, NULL); - motor_control(lMotor*m_speed,rMotor*m_speed); - while(1){ - //bt.printf("Pulses is: Revolutions is: \r\n"); - //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); +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; + +int main() +{ + initRobot(); + //motor_control((100),(-100)); + 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("K"); + } + } + } + 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); + buzzer=0; + //motor_control(lMotor*m_speed,rMotor*m_speed*0.8); + //motor_control(lMotor*m_speed,0); + 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.unlock(); - if(bt.readable()){ - c = bt.getc(); - if(c=='?') - { - bt.lock(); - bt.printf("EBOTDEMO\n"); - bt.unlock(); + // 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); } - /* int number = (c-'0'); - stdio_mutex.lock(); - freq = number; - stdio_mutex.unlock(); - motor_control(0,0); - - */} - if(ldrread1()>0.6) - { - //pl_buzzer(); - + + if(ldrread1()>0.6) { + } + if(ldrread2()>0.6) { + Led_on(); + } else { + Led_off(); + } + Thread::wait(20); } - if(ldrread2()>0.6) - { - Led_on(); - } - else - { - Led_off(); - } - //Thread::wait(1); - Thread::wait(50); - - } } \ No newline at end of file