Software system
- Algorithm diagram
The path planning of mobile robot is determined and
constructed like the Figure 1 and Figure 2.
Figure 1: Path working of robot when floor don’t
have obstacles
Figure 2:. Path working of robot when obstacle
detected
Base on the path planning, the algorithm used to control the
motion of mobile robot was built.
Algorithm diagram of Slave Chip:
Algorithm diagram of Master Chip:
- Coding
PID motor code:
void getMotorData()
{
speed_act1 =
((count1 - countAnt1)*(60*(1000/LOOPTIME)))/(424);
countAnt1 = count1;
speed_act2 = ((count2 -
countAnt2)*(60*(1000/LOOPTIME)))/(424);
countAnt2 = count2;
}
int updatePid1(int
command1, int targetValue1, int currentValue1)
{
float pidTerm1 = 0,
intergral1 =0, derivative1 = 0;
int error1=0;
static int last_error1=0;
error1 = abs(targetValue1) -
abs(currentValue1);
intergral1 = intergral1 + error1/10;
derivative1 = (error1-last_error1)*10;
pidTerm1 = (Kp1 * error1) + (Ki1 * intergral1)
+(Kd1 * derivative1);
last_error1 = error1;
return constrain(command1 + int(pidTerm1), 0,
255);
}
Sensor SRF05 code:
#define
ECHOPIN 2
#define TRIGPIN 3
#define ECHOPIN1 4
#define TRIGPIN1 5
int distance,distance1;
intbuf[3];
void setup() {
Serial.begin(9600);
pinMode(ECHOPIN, INPUT);
pinMode(TRIGPIN, OUTPUT);
pinMode(ECHOPIN1, INPUT);
pinMode(TRIGPIN1, OUTPUT);
}
void get_distance()
{ digitalWrite(TRIGPIN, LOW); // Set the trigger pin to low for 2uS
delayMicroseconds(2);
digitalWrite(TRIGPIN, HIGH); // Send a 10uS high to trigger ranging
delayMicroseconds(10);
digitalWrite(TRIGPIN, LOW); // Send pin low again
distance =
pulseIn(ECHOPIN, HIGH); // Read in
times pulse
distance =
distance/58;
delay(500);
//
Serial.print("distance: ");
Serial.print(distance);
}
void get_distance1()
{ digitalWrite(TRIGPIN1, LOW); // Set the trigger pin to low for 2uS
delayMicroseconds(2);
digitalWrite(TRIGPIN1, HIGH); // Send a 10uS high to trigger ranging
delayMicroseconds(10);
digitalWrite(TRIGPIN1, LOW); // Send pin low again
distance1 =
pulseIn(ECHOPIN1, HIGH); // Read in
times pulse
distance1 =
distance1/58;
delay(500);
//
Serial.print(" distance1:
"); Serial.println(distance1);
}
Sensor HMC5883l code:
void get_y()
{
if (i<=0)
{
Vector norm = compass.readNormalize();
y0 = norm.YAxis;
float heading = atan2(norm.YAxis, norm.XAxis);
float
declinationAngle = -0.025;
heading +=
declinationAngle;
if (heading < 0)
{ heading += 2 * PI; }
if (heading
> 2 * PI)
{
heading -= 2 * PI; }
angle0 = heading * 180/M_PI;
angle2 = angle0+90;
if (angle2 >= 360)
{ angle2=angle2-360;}
angle3=angle0+180;
if (angle3 >= 360)
{ angle3=angle3-360; }
angle4=angle0+270;
if (angle4 >= 360)
{ angle4=angle4-360; }
angle1 = angle0;
if (angle1 >= 360)
{ angle1=angle1-360;}
i = i+1;
}
if (i>=1)
{
Vector norm = compass.readNormalize();
y1 = norm.YAxis;
float heading = atan2(norm.YAxis, norm.XAxis);
float declinationAngle = -0.025;
heading += declinationAngle;
if
(heading < 0)
{ heading
+= 2 * PI; }
if (heading > 2 * PI)
{ heading -= 2 * PI; }
angle = heading * 180/M_PI;
i = i+1;
}
}
Code for Processing:
void cmd_process()
{ switch(count)
{
case
1: {
motor_forward();
break;
}
case 2: {
Serial.end();
motor_stop();
delay(500);
if (y1<(y0-40))
{ motor_right();
Serial.begin(9600);
break;
}
if (y1>=(y0-40))
{ motor_left();
Serial.begin(9600);
break;
}
}
case
3: {
Serial.end();
motor_stop();
delay(500);
motor_turnr();
Serial.begin(9600);
break;
}
case 4: {
Serial.end();
motor_stop();
delay(500);
motor_turnl();
Serial.begin(9600);
break;
}
default:{
motor_stop();
break;
}
}
}
The code for master and slave chip can be downloaded with this link below.
No comments:
Post a Comment