AForge.NET

  :: AForge.NET Framework :: Articles :: Forums ::

Help with self balancing robot.

The forum is to discuss topics related to robotics, like building robot, controlling it, its software and hardware, etc.

Help with self balancing robot.

Postby robodazed » Wed Apr 13, 2016 7:38 pm

It balances and moves back and forth, but I need it to move forward and avoid obstacles. What I'm I missing in the code, please help :?










#define GYRO IN_1
#define LEFT_MOTOR OUT_C
#define RIGHT_MOTOR OUT_A
#define MOTORS OUT_AC
#define WAIT_TIME 8 // for loop wait time in [ms]

#define WHEEL_RATIO_NXT1 1.0 // NXT 1.0 wheels (56x26 size)
#define WHEEL_RATIO_NXT2 0.7 // NXT 2.0 wheels (43x22 size)
#define WHEEL_RATIO_RCX 1.4 // RCX big wheels (81x15 size)

#define KGYROANGLE 7.5 // body integral gain [5.0, 10.0] [deg]
#define KGYROSPEED 1.15 // body proportional gain [1.2 or so] [deg/s]
#define KPOS 0.07 // motor proportional gain [non-zero] [deg]
#define KSPEED 0.1 // motor derivative gain [0.075, 0.2] [deg/s]

#define EMAOFFSET 0.0005 // Gyro offset compensation (low-pass filter constant)
#define TIME_FALL_LIMIT 1000 // Expiration time, robot fell
#define OFFSET_SAMPLES 100 // # of samples to use in calculating gyro offset

#define CONTROL_WAIT 25

// Global variables

long tCalcStart; // used to calculate interation time
float tInterval; // iteration time [sec]
float ratioWheel = WHEEL_RATIO_NXT2; // set for 43x22 sized wheels

float gOffset; // gyro offset value
float gAngleGlobal = 0; // angle calculated from gyro readings [deg]

float motorPos = 0; // accumulated motor position
long mrcSum = 0, mrcSumPrev; // used to calculate accumulated motor position
long mrcDeltaP3 = 0; // moving average to calculate accumulated motor position
long mrcDeltaP2 = 0;
long mrcDeltaP1 = 0;

void GetGyroOffset() { // compute offset by polling gyro and computing average
float gSum; // store sum of all raw gyro readings
int i; // dummy index variable
int g; // raw gyro reading [deg/s]
int gMin, gMax; // if gyro reading an outlier, then set at min or max value

ClearScreen();
TextOut(0, LCD_LINE4, "Gyro offset:");
TextOut(0, LCD_LINE5, "Lay robot flat");
Off(MOTORS); // ensure motors are off so they don't perturb gyro

do {
gSum = 0.0;
gMin = 1000;
gMax = -1000;
for (i=0; i<OFFSET_SAMPLES; i++) { // collect some gyro readings
g = SensorHTGyro(GYRO);
if (g > gMax) gMax = g;
if (g < gMin) gMin = g;
gSum = gSum + g;
Wait(5);
}
} while ((gMax - gMin) > 1); // Reject and sample again if range too large

gOffset = gSum/OFFSET_SAMPLES; // Compute average
} // end GetGyroOffset

void StartBeeps() { // 5-sec countdown until balance control begins
int c;

ClearScreen();
TextOut(20, LCD_LINE3, "Balancing in");
for (c=5; c>0; c--) { // countdown from 5 seconds
NumOut(47, LCD_LINE4, c);
PlayTone(440,100);
Wait(1000);
}
NumOut(47, LCD_LINE4, 0);
PlayTone(440,1000);
Wait(1000);
} // end of StartBeeps

void GetGyroData(float &gyroSpeed, float &gyroAngle)
{ // read gyro sensor, apply low-pass filter and return [deg/s] and [deg]
float gyroRaw; // raw gyro reading [deg/s]

gyroRaw = SensorHTGyro(GYRO);
gOffset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * gOffset; // low pass filter
gyroSpeed = gyroRaw - gOffset; // compute gyro speed [deg/s]
gAngleGlobal = gAngleGlobal + gyroSpeed*tInterval; // compute gyro angle [deg]
gyroAngle = gAngleGlobal;
} // end of GetGyroData

void GetMotorData(float &motorSpeed, float &motorPos)
{ // read motor encoder, compute accumulated angle and speed [deg], [deg/s]
long mrcLeft, mrcRight, mrcDelta; // mrc: motor rotation count

mrcLeft = MotorRotationCount(LEFT_MOTOR);
mrcRight = MotorRotationCount(RIGHT_MOTOR);

mrcSumPrev = mrcSum;
mrcSum = mrcLeft + mrcRight;

mrcDelta = mrcSum - mrcSumPrev;
motorPos = motorPos + mrcDelta; // accumulated motor position [deg]

// Implement moving average (use 4 readings) to compute motorSpeed [deg/s]
motorSpeed = (mrcDelta+mrcDeltaP1+mrcDeltaP2+mrcDeltaP3)/(4*tInterval);
mrcDeltaP3 = mrcDeltaP2;
mrcDeltaP2 = mrcDeltaP1;
mrcDeltaP1 = mrcDelta;
} // end of GetMotorData

void CalcInterval(long cLoop)
{ // compute loop iteration time [sec]
if (cLoop == 0) { // 1st time called this function called...
tInterval = 0.0055; // ...so arbitrarily set tInterval. 5.5 ms ok choice
tCalcStart = CurrentTick();
} else {
// Take average of number of times through the loop and use for tInterval
tInterval = (CurrentTick() - tCalcStart)/(cLoop*1000.0); // [sec]
}
} // end of CalcInterval

task main() {

SetSensorLowspeed(IN_4); // Initialize ultrasonic sensor

SetSensorHTGyro(GYRO); // Initialize gyro
Wait(50);
ResetRotationCount (OUT_ABC);
OnFwdSync (OUT_BC,75, 0);
until ((SensorUS (IN_4))< 20);
OffEx (OUT_BC, RESET_ALL);
GetGyroOffset(); // Get gyro offset
StartBeeps(); // Play 5-sec countdown beep before balancing starts

// when main ends, other tasks, like whipBalance, will start
} // end of main


task whipBalance() {
Follows(main);

float gyroSpeed, gyroAngle; // [deg/s] and [deg] from gyro
float motorSpeed; // [deg/s] from motor
int power; // power level to apply to motors
long tMotorPosOK; // timing variable to use for CurrentTick
long cLoop = 0; // used to compute interval time

ClearScreen();
TextOut(0, LCD_LINE4, "Balancing");
tMotorPosOK = CurrentTick();
ResetRotationCount(LEFT_MOTOR); // reset motors ensures angle starts at 0 deg
ResetRotationCount(RIGHT_MOTOR);

while(true) {
CalcInterval(cLoop++); // compute time elapsed between samples
GetGyroData(gyroSpeed, gyroAngle); // body [deg/s] and [deg]
GetMotorData(motorSpeed, motorPos); // motor [deg/s] and [deg]

// PID equation
power = (KGYROSPEED * gyroSpeed + // body vel [deg/s]
KGYROANGLE * gyroAngle) / ratioWheel + // body pos [deg]
KPOS * motorPos + // motor pos [deg]
KSPEED * motorSpeed; // motor vel [deg/s]

if (abs(power) < 100)
tMotorPosOK = CurrentTick();
OnFwd(MOTORS, power); // apply power to both motors

if ((CurrentTick()-tMotorPosOK) > TIME_FALL_LIMIT) { // robot fell
break;
}
Wait(WAIT_TIME); // still balanced, so repeat loop

} // end of while(true). Exits if robot fell

Off(MOTORS);
TextOut(0, LCD_LINE4, "Oops... I fell");
TextOut(0, LCD_LINE8, "tInt ms:");
NumOut(6*8, LCD_LINE8, tInterval*1000);
StopAllTasks(); // kill all running tasks

} // end task whipBalance


task ultraSoundResponse() {
Follows(main);

byte usSensorValue;

while(true) {
// read us sensor
usSensorValue = SensorUS(IN_4); // US sensor value is 0 to 254 [cm]
if(usSensorValue < 30) {
PlayTone(TONE_B3, MS_50); // beep if WhIP within 30 cm of object
}
Wait(100); // wait a bit, say 100 ms, before polling US sensor again
} // end while

} // end ultraSoundResponse
robodazed
 
Posts: 1
Joined: Wed Apr 13, 2016 7:34 pm



Return to Robotics

cron