So, i used a SR04 sonar module instead with a really simple approach. i held the robot in its upright posisition on a flat surfaced and found the distance to the table. The motor control loops throught the following for every sonar read(every 30-50ms):
If the sonar value is too low(close to the table) turn the servos to compensate
If too far from table, then turn servos the other way.
If sonar value balanced, lock motors.
I used my old Boe-bot frame so it wasn't very balanced in the first place(made out of steel), then i added the battery pack and that totally messed it up in terms of weight distribution(messy as heck too :)). I will soon make a custom acrylic or light wood frame for it so that it will be more balanced. i think if i want to drive it around(maybe with that TV remote :)) im going to have to make this control setup a little more complex.
Pics:
Sonar Module |
Code:
//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0;
int RmtrSpd=0;
#define LServoMtrPin 2
#define RServoMtrPin 3
#include <Servo.h>
Servo LServoMtr;
Servo RServoMtr;
//----------------------------------------------------------------//
//Sonar------------------------------------------------------------
float dist=0;
int trig=6;
int echo=7;
float SonarVal=0;
#define BlncPnt 850 //find this !!!
//----------------------------------------------------------------//
void setup()
{
Serial.begin(9600);
pinMode(echo, INPUT);
pinMode(trig, OUTPUT);
LServoMtr.attach(LServoMtrPin);
LServoMtr.write(90);
RServoMtr.attach(RServoMtrPin);
RServoMtr.write(90);
}
void loop()
{
//Sonar------------------------------------------------------------
digitalWrite(trig, HIGH);
delayMicroseconds(10);
digitalWrite(trig, LOW);
SonarVal= pulseIn(echo, HIGH);
Serial.println(SonarVal);
//---Balance-Test---------------------------------------------
if(SonarVal > BlncPnt+25)
{
LServoMtr.write(0);
RServoMtr.write(180);
}
else if(SonarVal < BlncPnt-25)
{
LServoMtr.write(180);
RServoMtr.write(0);
}
else
{
LServoMtr.write(90);
RServoMtr.write(90);
}
delay(45);
}
No comments:
Post a Comment