EdgeBotix
Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
Revision 3:d1197b5ea68a, committed 2014-10-12
- Comitter:
- Throwbot
- Date:
- Sun Oct 12 23:27:43 2014 +0000
- Parent:
- 2:0f80c8a1c236
- Child:
- 4:81b0de07841f
- Commit message:
- Ebot demo working
Changed in this revision
--- a/Drivers/ultrasonic/ultrasonic.cpp Sun Oct 12 13:33:19 2014 +0000
+++ b/Drivers/ultrasonic/ultrasonic.cpp Sun Oct 12 23:27:43 2014 +0000
@@ -68,7 +68,19 @@
}
void ultrasonic_run()
{
- SRX=1;
+ SRX=1;
+ wait_us(30);
+ SRX=0;
+ Thread::wait(150);
+ stdio_mutex.lock();
+ UL_L=sensorvalue(uL);
+ UL_rL=sensorvalue(urL);
+ UL_B=sensorvalue(uB);
+ UL_rR=sensorvalue(urR);
+ UL_R=sensorvalue(uR);
+ UL_F=sensorvalue(uF);
+ stdio_mutex.unlock();
+ /* SRX=1;
wait_ms(5);
stdio_mutex.lock();
UL_rR=sensorvalue(urR);
@@ -113,5 +125,5 @@
stdio_mutex.lock();
UL_B=sensorvalue(uB);
stdio_mutex.unlock();
- SRX=0;
+ SRX=0; */
}
--- a/Robot.lib Sun Oct 12 13:33:19 2014 +0000 +++ b/Robot.lib Sun Oct 12 23:27:43 2014 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/teams/Abhishek/code/Robot_Original/#37a19a9e5f2c +http://developer.mbed.org/teams/Abhishek/code/Robot_Original/#3e3314102e56
--- a/main.cpp Sun Oct 12 13:33:19 2014 +0000
+++ b/main.cpp Sun Oct 12 23:27:43 2014 +0000
@@ -8,33 +8,29 @@
#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;
+int Error_Right=0;
+int Error_Left=0;
+float L_Kp=0.1;
+float R_Kp=0.1;
+int previous_Linput= 0 ;
+int previous_Rinput= 0 ;
RtosTimer *Motor_controller_Timer;
void Motorcontroller(void const *args)
{
- Current_Right_pulse= right.getPulses();
- Current_Left_pulse=left.getPulses();
+ Current_Right_pulse= right.getPulses()/5;
+ Current_Left_pulse=left.getPulses()*(-1)/5;
Change_Right_pulse=Current_Right_pulse- Previos_Right_pulse;
Change_Left_pulse=Current_Left_pulse- Previous_Left_pulse;
@@ -42,25 +38,35 @@
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;
+ Error_Right=(Rmotor_speed- (Change_Right_pulse*time_factor) );
+ Error_Left=(Lmotor_speed- (Change_Left_pulse*time_factor));
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)
+ else if (-11<Linput && Linput<11)
Linput= 0;
if(Rinput>100)
Rinput=100;
else if(Rinput<-100)
Rinput= -100;
- else if (-21<Rinput && Rinput<21)
+ else if (-11<Rinput && Rinput<11)
Rinput= 0;
+ if (Linput== previous_Linput && Rinput ==previous_Rinput) {
+ } else {
+ previous_Linput= Linput ;
+ previous_Rinput= Rinput ;
+ motor_control(-Rinput,-Linput);
+ }
+ /*
+ bt.lock();
+ bt.printf(">>D;%d;%d;%d;%d;%d;%d;%d;%d;\r\n",
+ Rmotor_speed,Lmotor_speed,Error_Right ,Error_Left,\
+ Rinput,Linput,Change_Right_pulse,Change_Left_pulse);
+ bt.unlock();
+ */
}
@@ -97,7 +103,7 @@
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);
+ Motor_controller_Timer->start(time_int);
while(1) {
if(ldrread1()>0.6) {
}
--- a/shell/bt_shell.cpp Sun Oct 12 13:33:19 2014 +0000
+++ b/shell/bt_shell.cpp Sun Oct 12 23:27:43 2014 +0000
@@ -17,6 +17,7 @@
int Lspeed=1;
int Rspeed=1;
int k=0;
+int send_thetha=0;
int char_received=0;
DigitalOut myreset(PTA20);
Timeout reset_pgm;
@@ -25,10 +26,10 @@
{
bt.lock();
stdio_mutex.lock();
- heading=heading*11.375;
+ send_thetha=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,\
+ (send_thetha)/256,(send_thetha)%256,\
stall, bump,\
UL_rR/256,UL_rR%256,\
UL_R/256,UL_R%256, \
@@ -55,29 +56,31 @@
linear_velocity_direction= buffer[2];
rotational_velocity_value = buffer[3]<<8|buffer[4];
rotational_velocity_direction= buffer[5];
- if( linear_velocity_direction==0x01)
- {
- Lspeed=-lMotor;
- Rspeed=-rMotor;
+
+ if( linear_velocity_direction==0x01) {
+ linear_velocity_value=linear_velocity_value;
+ linear_velocity_value=linear_velocity_value;
} else if( linear_velocity_direction==0x10) {
- Lspeed=+lMotor;
- Rspeed=+rMotor;
+ linear_velocity_value=-linear_velocity_value;
+ linear_velocity_value=-linear_velocity_value;
}
if( rotational_velocity_direction==0x01 && rotational_velocity_value !=0) {
- motor_control(Lspeed*60,Rspeed*0);
-
+ rotational_velocity_value=rotational_velocity_value*M_PI*50/180;
+ Rmotor_speed=linear_velocity_value+rotational_velocity_value;
+ Lmotor_speed=linear_velocity_value-rotational_velocity_value;
} else if( rotational_velocity_direction==0x10 && rotational_velocity_value !=0) {
- motor_control(Lspeed*0,Rspeed*60);
- } else{
- motor_control((Lspeed*linear_velocity_value/4),(Rspeed*linear_velocity_value/4));
-
- }
+ rotational_velocity_value=rotational_velocity_value*M_PI*50/180;
+ Rmotor_speed=linear_velocity_value-rotational_velocity_value;
+ Lmotor_speed=linear_velocity_value+rotational_velocity_value;
+ } else {
+ Rmotor_speed=linear_velocity_value;
+ Lmotor_speed=linear_velocity_value;
+ }
} else if(buffer[0] == 'P' && buffer[1]== 'O') {
- software_interuupt=1;
+ software_interuupt=1;
myreset=0;
- }
- else
- {
+ }
+ else {
bt.rxBufferFlush();
}
}