EdgeBotix
Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
Diff: main.cpp
- Revision:
- 1:1da89c13dfa1
- Parent:
- 0:43331220df0d
- Child:
- 2:0f80c8a1c236
--- 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