 | | | | | | |
| |
|
This code was working last night, but I removed some superfluous functions
copied with the set (such as moveBackward() and pivotRight()) since they
were not being used. All of a sudden, the robot turns okay, but never moves
forward. It just sits there with the 'turn motor' on B chattering. If the
axle of the rotation sensor attached to the drive system is rotated a little
bit, the motor starts turning the robot in spurts. Almost as if the
rotation sensor is going nutso or something. A left turn is negative units
for the rotation sensor.
I reinstalled a program that uses the same drive system and it works, so
there is no mechanical or electrical failure and nothing (hopefully) wrong
with the legOS firmware. I had to reinstall it a couple times yesterday due
to motor stalls (when it should have stopped due to no rotation change) on
this previous program, but it worked after this. Could the sensor be going
bad? If so, I do have another that can be swapped out for a test.
If not, can anyone detect any problems in this code? It is almost identical
to the code used by the working program (which is much more complex even).
************************************************************************
#include <stdlib.h>
#include <unistd.h>
#include <conio.h>
#include <dlcd.h>
#include <dsensor.h>
#include <dmotor.h>
// Sensor definitions
#define TURN_DETECTOR SENSOR_1
#define RANGE_DETECTOR SENSOR_3
// Sensor value definitions
#define TURN_ANGLE ROTATION_1
#define DISPLACEMENT TURN_ANGLE
#define RANGE_DISTANCE LIGHT_3
// Turn Angle units - approx. (134 run per robot rotation : 1 run = 2.7 deg)
#define TA_RUN(d) ((long)((long)(d)*134)/360)
#define TA_DEG(u) ((long)((long)(u)*360)/134)
// 'D' is maximum field effect range in 1/2 inch units
#define MAX_DISTANCE 48
#define MAX_VELOCITY MAX_SPEED
// vector structure definition
typedef struct
{
int magnitude;
int direction;
} vector;
//*** Move Forward ***
void moveForward(unsigned char speed)
{
motor_a_speed(speed);
motor_a_dir(fwd);
motor_b_dir(off);
}
//*** Pivot Left ***
void pivotLeft(unsigned char speed)
{
motor_b_speed(speed);
motor_a_dir(off);
motor_b_dir(rev);
}
//*** Stop motion (brake) ***
void stopMotion()
{
motor_a_dir(brake);
motor_b_dir(brake);
}
//*** Reset Turn Detector ***
void resetTurnDetector()
{
// reset rotation sensor to 0 degrees cumulated
ds_rotation_set(&TURN_DETECTOR,0);
msleep(100);
}
//**********************************************
vector Vout;
void turn(int angle)
{
// convert degrees to Turn Angle rotation units
angle = TA_RUN(angle);
pivotLeft(MAX_SPEED);
while(TURN_ANGLE > angle);
stopMotion();
resetTurnDetector();
}
void repulsive(int d, int D)
{
if(d <= D)
{
// turn around
Vout.direction = -180;
// linear dropoff
Vout.magnitude = ((D-d)<<7)/D;
}
else
{
Vout.direction = 0;
Vout.magnitude = 0;
}
}
int main(int argc, char** argv)
{
// initialize system
// -- initialize motors
ds_active(&TURN_DETECTOR);
ds_rotation_on(&TURN_DETECTOR);
resetTurnDetector();
// -- initialize sonar
ds_active(&RANGE_DETECTOR);
while(1)
{
// runaway
repulsive(RANGE_DISTANCE, MAX_DISTANCE);
turn(Vout.direction);
moveForward((Vout.magnitude * MAX_VELOCITY)>>7);
}
return 0;
}
| | | | | | | | | | | | | | |
In lugnet.robotics.rcx.legos, Robert Templeton writes:
Nothing like talking to yourself :), but I did finally find the problem and
it is in the code. It isn't always what's there, but what's not. Here's
the offending section:
> //*** Move Forward ***
> void moveForward(unsigned char speed)
> {
> motor_a_speed(speed);
> motor_a_dir(fwd);
> motor_b_dir(off);
> }
Instead of varying the speed, I had been varying the distance with this
function wrapper:
void forward(unsigned int distance)
{
distance <<= 2;
moveForward(MAX_SPEED);
while(DISPLACEMENT < distance);
//*******************
stopMotion();
resetTurnDetector();
//********************
}
When I switched to speed variance, I figured I didn't need all of that
"useless" code in the forward() function. Wrong!!! The lack of those two
comment-surrounded lines is the cause of the problem.
Say "Goodnight, Gracie."
Goodnight, Gracie...
D'oh!
Robert Templeton
| | | | | | |
