I wrote the following code using easyC for Vex.
It's a function, it's supposed to take the angle I give it and turn in that direction. If I give it a positive angle, it turns to the right. I'm trying to make it turn to the left if I give it a negative angle, but it ain't happening. For some reason it simply turns to the right again. Any ideas?
Code:
#include "Main.h"
void turn ( int angle )
{ // Turns the robot a set number of degrees
int done = 0;
int abs;
angle = angle/4 ; //For the optical sensors, one turn is 90 pulses.
if ( angle < 0 )
{
abs = -angle ;
}
else
{
abs = angle ;
}
PresetEncoder ( L_OPTICAL_SENSOR_CHANNEL , Left_Optical_Sensor ) ;
PresetEncoder ( R_OPTICAL_SENSOR_CHANNEL , Right_Optical_Sensor ) ;
StartEncoder ( L_OPTICAL_SENSOR_CHANNEL ) ;
StartEncoder ( R_OPTICAL_SENSOR_CHANNEL ) ;
while ( done != 2 )
{
Left_Optical_Sensor = GetEncoder ( L_OPTICAL_SENSOR_CHANNEL ) ;
Right_Optical_Sensor = GetEncoder ( R_OPTICAL_SENSOR_CHANNEL ) ;
if ( Left_Optical_Sensor < abs )
{
if ( angle > 0 & angle <= 90 )
{
SetMotor ( L_MOTOR , 255 ) ;
}
if ( angle >= -90 & angle < 0 )
{
SetMotor ( L_MOTOR , 0 ) ;
}
}
else
{
SetMotor ( L_MOTOR , 127 ) ;
done = done + 1 ;
StopEncoder ( L_OPTICAL_SENSOR_CHANNEL ) ;
}
if ( Right_Optical_Sensor < abs )
{
if ( angle > 0 & angle <= 90 )
{
SetMotor ( R_MOTOR , 255 ) ;
}
if ( angle >= -90 & angle < 0 )
{
SetMotor ( R_MOTOR , 0 ) ;
}
}
else
{
SetMotor ( R_MOTOR , 127 ) ;
done = done + 1 ;
StopEncoder ( R_OPTICAL_SENSOR_CHANNEL ) ;
}
}
return ;
}