I installed a gyro on our robot and tried to code it to make the wheel motors reverse if it tilted backwards but cant seem to get it to work. I am not getting any errors when I compile the program so it appears as though my syntax is right, but when I test it nothing happens. Below is the code I am using in RobotC. Thanks in advance!!
// Begin User Control.
task usercontrol()
{
//Completely clear out any previous sensor readings by setting the port to “sensorNone”
SensorType[in1] = sensorNone;
wait1Msec(500);
//Reconfigure Analog Port 1 as a Gyro sensor and allow time for ROBOTC to calibrate it
SensorType[in1] = sensorGyro;
wait1Msec(1000);
//Adjust SensorScale to correct the scaling for your gyro
SensorScale[in1] = 90;
//Adjust SensorFullCount to set the “rollover” point. 3600 sets the rollover point to +/-3600
SensorFullCount[in1] = 3600;
//Specify the absolute number of degrees the robot is allowed to tilt (1 degree = 10, or 900 = 90 degrees)
int degrees10 = 150; //Allows no more than 15 degrees of backwards tilt.
//While the absolute value of the gyro is greater than the allowed level of tilting…
while(abs(SensorValue[in1]) > degrees10)
//…back up to correct backwards tilting.
motor[rightMotor] = -100;
motor[leftMotor] = -100;
{
{
This part right here would explain the nothing happening:
//While the absolute value of the gyro is greater than the allowed level of tilting...
while(abs(SensorValue[in1]) > degrees10)
//...back up to correct backwards tilting.
motor[rightMotor] = -100;
motor[leftMotor] = -100;
{
{
For one, the ending braces should be closing braces. Change them to } instead of {.
And if you want multiple lines in your while loop to run, you must enclose them in braces {}.
So that snippet should be:
//While the absolute value of the gyro is greater than the allowed level of tilting...
while(abs(SensorValue[in1]) > degrees10)**{**
//...back up to correct backwards tilting.
motor[rightMotor] = -100;
motor[leftMotor] = -100;
**}
}
}**
No luck so far. Below is my entire program instead of just a snippet of the code. I just installed the gyro and encoders. I will tackle the encoders later since I think I can figure them out myself (I hope).
#pragma config(Sensor, in1, Gyro, sensorGyro)
#pragma config(Sensor, dgtl1, encoderRight, sensorQuadEncoder)
#pragma config(Sensor, dgtl3, encoderLeft, sensorQuadEncoder)
#pragma config(Motor, port2, rightMotor, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor, port4, armMotorlleft, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor, port5, armMotorright, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor, port6, intakeMotor, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor, port7, leftMotor, tmotorServoContinuousRotation, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(VEX)
//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(90)
#include "Vex_Competition_Includes.c"
//Main competition background code...do not modify!
void pre_auton()
{
bStopTasksBetweenModes = true;
}
task autonomous()
//Begin music to play during autonomous mode.
{
//Beging autonomous mode.
motor[armMotorlleft] = 100;
motor[armMotorright] = 100;
wait1Msec(1000);
motor leftMotor] = 90; //Left motor started at 90% to cancel yaw
motor rightMotor ] = 100; // caused by slight delay in right motors starting.
motor leftMotor ] = 100;
wait1Msec(1000); // 1 second wait
motor leftMotor] = -100; // Begin left turn execution
wait1Msec(500);
motor rightMotor ] = 100; // Move over 1 tile left
motor leftMotor ] = 100;
wait1Msec(700);
motor[rightMotor] = -100; // Begin right turn execution
wait1Msec(525); // Wait 0.525 seconds
motor[rightMotor] = 100; // Move towards goal
motor[leftMotor] = 100;
wait1Msec(1000); // Move towards goal for 1 second
motor[rightMotor] = 0; // Stop at goal
motor[leftMotor] = 0;
wait1Msec(500); // Wait 0.5 seconds
motor[leftMotor] =80; // Begin movement towards goal
motor[rightMotor] = 80;
wait1Msec(300); // Wait 0.3 seconds
motor[rightMotor] = 0;
motor[leftMotor] = 0;
motor[intakeMotor] = -100; // Eject preloaded sack from hopper.
wait1Msec(2000); // Wait 2 seconds
motor[intakeMotor] = 0;
motor[rightMotor] = -100; // Back away from goal
motor[leftMotor] = -100;
wait1Msec(750); // Wait 0.75 seconds
motor[rightMotor] = 0; // Come to stop
motor[leftMotor] = 0;
motor[armMotorlleft] = -100; // Lower arm to ground
motor[armMotorright] = -100;
wait1Msec(300); // Wait 0.3 seconds
motor[armMotorlleft] = 0; // Stop arm motors, end autonomous routine stage 1.
motor[armMotorright] = 0;
motor leftMotor] = 70; //Left motor started at 70% to cancel yaw
motor rightMotor ] = 80; //caused by slight delay in right motors starting.
motor leftMotor ] = 80;
motor[intakeMotor] = 100;
wait1Msec(1000); // move foreward to pick up sacks from below goal.
motor[intakeMotor] = 0; // Stop intake motors and prepare for backwards movement.
motor[leftMotor] = -80; // Raise arms prior to moving towards goal.
motor[rightMotor] = -80;
wait1Msec(1000); // Stop and prepare to raise arm.
motor[leftMotor] = 0;
motor[rightMotor] = 0;
motor[armMotorlleft] = 100; // Raise arms fully before approaching goal
motor[armMotorright] = 100;
wait1Msec(1000); // Allow 1 second for arm to raise.
motor[leftMotor] = 70; // Begin movement towards scoring goal
motor[rightMotor] = 80;
motor[leftMotor] = 80;
wait1Msec(1000);
motor[rightMotor] = 0; // Arrive at goal, stop wheel motors.
motor[leftMotor] = 0;
motor[intakeMotor] = -100; // Eject sacks into goal.
wait1Msec(1500);
motor[intakeMotor] = 0; // Stop ejection of sacks prepare to park.
motor[leftMotor] = -100;
motor[rightMotor] = -100;
wait1Msec(500); // Begin to move away from goal with arms raised.
motor[armMotorlleft] = -100; // Lower arms to ground level.
motor[armMotorright] = -100;
wait1Msec(500); // Stop all motors.
motor[armMotorlleft] = 0;
motor[armMotorright] = 0;
motor[leftMotor] = 0;
motor[rightMotor] = 0; // End autonomous mode. BAZINGA!
// ***NOTE***
// This autonomous routine is .575 seconds longer than the
// autonomous period of 15 seconds but should complete all
// tasks other than completely lowering the basket prior to
// the end of the 15 second autonomous portion of competition.
}
// Begin User Control.
task usercontrol()
{
//Completely clear out any previous sensor readings by setting the port to "sensorNone"
SensorType[in1] = sensorNone;
wait1Msec(500);
//Reconfigure Analog Port 1 as a Gyro sensor and allow time for ROBOTC to calibrate it
SensorType[in1] = sensorGyro;
wait1Msec(1000);
//Adjust SensorScale to correct the scaling for your gyro
SensorScale[in1] = 90;
//Adjust SensorFullCount to set the "rollover" point. 3600 sets the rollover point to +/-3600
SensorFullCount[in1] = 3600;
//Specify the absolute number of degrees the robot is allowed to tilt (1 degree = 10, or 900 = 90 degrees)
int degrees10 = 150; //Allows no more than 15 degrees of backwards tilt.
//While the absolute value of the gyro is greater than the allowed level of tilting...
while(abs(SensorValue[in1]) > degrees10)
//...back up to correct backwards tilting.
motor[rightMotor] = -100;
motor[leftMotor] = -100;
{
{
}
}
while(1 == 1)
{
//Driving Motor Control
motor[leftMotor] = vexRT[Ch3] / 2;
motor[rightMotor] = vexRT[Ch2] / 2;
//Arm Control
if(vexRT[Btn5U] == 1)
{
motor[armMotorlleft] = 100;
motor[armMotorright] = 100;
}
else if(vexRT[Btn5D] == 1)
{
motor[armMotorlleft] = -100;
motor[armMotorright] = -100;
}
else
{
motor[armMotorlleft] = 0;
motor[armMotorright] = 0;
}
//Intake Roller
if(vexRT[Btn6U] == 1)
{
motor[intakeMotor] = 0;
}
else if(vexRT[Btn6D] == 1)
{
motor[intakeMotor] = -100;
}
else
{
motor[intakeMotor] = 0;
}
}
}
//Completely clear out any previous sensor readings by setting the port to "sensorNone"
SensorType[in1] = sensorNone;
wait1Msec(500);
//Reconfigure Analog Port 1 as a Gyro sensor and allow time for ROBOTC to calibrate it
SensorType[in1] = sensorGyro;
wait1Msec(1000);
//Adjust SensorScale to correct the scaling for your gyro
SensorScale[in1] = 90;
//Adjust SensorFullCount to set the "rollover" point. 3600 sets the rollover point to +/-3600
SensorFullCount[in1] = 3600;
//Specify the absolute number of degrees the robot is allowed to tilt (1 degree = 10, or 900 = 90 degrees)
int degrees10 = 150; //Allows no more than 15 degrees of backwards tilt.
//While the absolute value of the gyro is greater than the allowed level of tilting...
while(abs(SensorValue[in1]) > degrees10)
//...back up to correct backwards tilting.
motor[rightMotor] = -100;
motor[leftMotor] = -100;
{
{
}
}
You start of with gyro initialization code then start talking about tilt, this is an accelerometer function. Do you have a gyro (which measure yaw only) or a 3 axis accelerometer ?
If you have the gyro, don’t use these calls, they are not necessary.
//Adjust SensorScale to correct the scaling for your gyro
SensorScale[in1] = 90;
//Adjust SensorFullCount to set the "rollover" point. 3600 sets the rollover point to +/-3600
SensorFullCount[in1] = 3600;
[quote=jpearman;326215
]
I guess I purchased the wrong sensor I did mount the Gyro vertically so it could measure tilt. What I am trying to do is keep the robot from tipping backwards. We have not had it happen yet, but our driver sometimes pays more attention to unloading sacks than stopping forward movement and has come close. I want to try and make the motors run backwards when it gets tilted to far to auto correct.
Do I need to go buy an accelerometer or can this be done with a gyro?
[/quote]
Well I just tried this and the gyro will measure angle if mounted vertically and rotated around the axis of the mounting holes. Download the library from the link I posted and then try this small test program, use the Debugger LCD simulation if you don’t have a real LCD. I was surprised it worked, and there may be other side effects, but give it a go.
#pragma config(Sensor, in1, GyroInput, sensorGyro)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
// edit this path to point at the library if necessary
#include "GyroLib.c"
task main()
{
bLCDBacklight = true;
GyroInit( GyroInput );
while(1)
{
GyroDebug(0);
wait1Msec(100);
}
}
Thanks! I will also order an accelerometer and pick it up later this week We have a league competition tomorrow so I loaded our previous program on it which has worked great so far. The gyro is just so we have a sensor more or less, and the encoder are so we can score more than our usual one sack in autonomous mode. We have been using time in motion for our autonomous and it scores every time, but I would really like to snatch up those 5 sacks the robot is going past on the way to the scoring trough.
As always I appreciate your help and the fact that you constantly step up to the plate to help others! This forum has been the single greatest factor in our success aside from some really sharp kid’s.
I think the gyro may do what you want, the accelerometer can be even trickier to work with than the gyro, I have one but have never done much with it. The anti-tipping code is on my to do list but it’s going to have to wait until after April before I have any time to work on that.
Your original code in post #1 & #3 was never going to work. Stephen’s input was along the right lines in that your initial “while” loop did not contain any statement to run, however, even with his changes that code would only run once. Your code needs to be within the while(forever) loop where you are reading the joystick and everything else, something like this (expressed as a mixture of C and pseudo code).
// Begin User Control.
task usercontrol()
{
int left_motor_value;
int right_motor_value;
float angle;
// init everything
Do_all_initialization();
while(1 == 1)
{
//Driving Motor Control
left_motor_value = vexRT[Ch3] / 2;
right_motor_value = vexRT[Ch2] / 2;
// get the tilt of the robot
angle = GetGyroAngle();
// check angle - depends on how it is mounted
// angle is in range 0 - 360 (180 would be upside down)
// 350 would be tipping forwards so limit the test
if(( angle > 15.0 ) && (angle < 180.0))
{
// modify the motor commands if it is tipping backwards
// you may want to check for forward or backward motion as well
left_motor_value = -100;
right_motor_value = -100;
}
// send to motors
motor[leftMotor] = left_motor_value;
motor[rightMotor] = right_motor_value;;
//Arm Control
// Do your arm control
//Intake Roller
// Do your intake control
}
}
//While the absolute value of the gyro is greater than the allowed level of tilting...
while(abs(SensorValue[in1]) > degrees10)
//...back up to correct backwards tilting.
motor[rightMotor] = -100;
motor[leftMotor] = -100;
{
{
}
}
Was what would not work. First of all, the curly brackets were in the wrong places. It should look like this:
//While the absolute value of the gyro is greater than the allowed level of tilting...
while(abs(SensorValue[in1]) > degrees10)
**{**
//...back up to correct backwards tilting.
motor[rightMotor] = -100;
motor[leftMotor] = -100;
**}**
Also, secondly, as tutman96 and John (jpearman) both said, you need to run this continually inside of a “while(true)” (or "while(1==1), or even just “while(1)” – anything that is always true) loop. Otherwise, it will only check once. So you code should then look like this:
//While the absolute value of the gyro is greater than the allowed level of tilting...
**while(true)
{**
while(abs(SensorValue[in1]) > degrees10)
{
//...back up to correct backwards tilting.
motor[rightMotor] = -100;
motor[leftMotor] = -100;
}
**}**
I did mount the Gyro vertically so it could measure tilt. What I am trying to do is keep the robot from tipping backwards. We have not had it happen yet, but our driver sometimes pays more attention to unloading sacks than stopping forward movement and has come close. I want to try and make the motors run backwards when it gets tilted to far to auto correct.