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