| csshakka |
10-02-2009 22:14 |
Program feedback/suggestions
1 Attachment(s)
Any suggestions on what i could change in my program? Nothing too complicated, but this is my first program and i'm unsure if i did things correctly or if they could be redone in a better way. Thanks for feedback.
Code:
/*
* Drive:
* Tank drive
* Triggers activate break
*
* Manipulations:
* Y-axis controls belt speed
* X-axis rotates turret
* Left/Right buttons on top also rotate turret
* Up button on top throttles belt motor(s) at full speed
* If trigger is held, both belts are controlled
* If trigger is released, only lower belt is controlled
*/
#include "WPILib.h"
class RobotMain : public SimpleRobot
{
// Drive motors
RobotDrive *robot;
// Joysticks
Joystick *leftStick;
Joystick *rightStick;
Joystick *manipStick;
// Manipulation motors
Jaguar *lowerBelt;
Victor *upperBelt;
Jaguar *turret;
// Gyro
Gyro *gyro;
// Accelerometer
Accelerometer *accl;
public:
// Use to configure each PWM with the correct motor
// Format: Motor = PWM; (left drive motor is on PWM 1)
const static int
leftdriveMotor = 1,
rightdriveMotor = 2,
lowerbeltMotor = 3,
upperbeltMotor = 4,
turretMotor = 5,
// Joysticks and USB ports
// Format: Joystick = USB Port; (left joystick is in USB 2)
leftJoystick = 2,
rightJoystick = 1,
manipJoystick = 3,
// Gyro & Accelerometer
// Format: Sensor = Analog Channel;
gyroAnalogChannel = 1,
accelAnalogChannel = 2;
// For accelerometer get value function
const static float
Gs = 0,
// Zero g voltage for accelerometer
zerogVoltage = 0.5,
// Sensitivity of accelerometer in Volts per G
acclSensitivity = 0.5,
// Gyro sensitivity (volts/degree/second)
gyroSensitivity = 0.125,
// Degrees to turn in autonomous mode
autonTurnDegrees = 45.0;
RobotMain(void)
{
// Creates drive train objects
robot = new RobotDrive(leftdriveMotor, rightdriveMotor);
// Joystick objects
leftStick = new Joystick(leftJoystick);
rightStick = new Joystick(rightJoystick);
manipStick = new Joystick(manipJoystick);
// Manipulation motor objects
lowerBelt = new Jaguar(lowerbeltMotor);
upperBelt = new Victor(upperbeltMotor);
turret = new Jaguar(turretMotor);
// Gyro object
gyro = new Gyro(gyroAnalogChannel);
// Accelerometer object
accl = new Accelerometer(accelAnalogChannel);
// sets all motors to zero
robot->TankDrive(0.0, 0.0);
lowerBelt->Set(0.0);
upperBelt->Set(0.0);
turret->Set(0.0);
// Get initial value for accelerometer
// Sets gyro to zero and declares it sensitivity
gyro->Reset();
gyro->SetSensitivity(gyroSensitivity);
accl->SetZero(zerogVoltage);
accl->SetSensitivity(acclSensitivity);
GetWatchdog().SetExpiration(0.1);
}
~RobotMain()
{
delete robot;
delete lowerBelt;
delete upperBelt;
delete turret;
delete leftStick;
delete rightStick;
delete manipStick;
delete accl;
delete gyro;
}
void Autonomous(void)
{
GetWatchdog().SetEnabled(false);
// sets all motors to zero
robot->SetLeftRightMotorSpeeds(0.0, 0.0);
lowerBelt->Set(0.0);
upperBelt->Set(0.0);
turret->Set(0.0);
// Get initial value for accelerometer
// Sets gyro to zero and declares it sensitivity
gyro->Reset();
gyro->SetSensitivity(gyroSensitivity);
accl->SetZero(zerogVoltage);
accl->SetSensitivity(acclSensitivity);
// stores turn degrees into writable variable
float TurnDegrees = autonTurnDegrees;
// if it is outside of 10º plus or minus from desired angle
// robot will spin, printing out values every 10th of a second
if (TurnDegrees < 0) TurnDegrees += 360;
while (gyro->GetAngle() <= (TurnDegrees - 10) ||
gyro->GetAngle() >= (TurnDegrees + 10))
{
robot->TankDrive(0.25, 0.0);
printf("Accel reads: %f\n", accl->GetAcceleration());
printf("Gyro reads: %f\n", gyro->GetAngle());
Wait(0.1);
}
// stops motors, waits half a second, then accelerates forward
// over a course of 5 seconds, then stops the motors
robot->TankDrive(0.0, 0.0);
Wait(0.5);
for (int i = 0; i <= 10; i++)
{
robot->TankDrive((i/10), (i/10));
Wait(.5);
}
robot->TankDrive(0.0, 0.0);
}
void OperatorControl(void)
{
GetWatchdog().SetEnabled(true);
// Initializes all motors to zero
robot->TankDrive(0.0, 0.0);
lowerBelt->Set(0.0);
upperBelt->Set(0.0);
turret->Set(0.0);
// Get initial value for accelerometer
// Sets gyro to zero and declares it sensitivity
gyro->Reset();
gyro->SetSensitivity(gyroSensitivity);
accl->SetZero(zerogVoltage);
accl->SetSensitivity(acclSensitivity);
// Prints initial values for gyro and accelerometer
printf("Accel reads: %f\n", accl->GetAcceleration());
printf("Accel reads: %f\n", gyro->GetAngle());
while (IsOperatorControl())
{
GetWatchdog().Feed();
// Drive Train Code Begin
// Triggers break on either right or left set of wheels
// Otherwise runs standard tank drive
if (leftStick->GetTrigger())
robot->TankDrive(0.0, rightStick->GetY());
else if (rightStick->GetTrigger())
robot->TankDrive(leftStick->GetY(), 0.0);
else if (leftStick->GetTrigger() && rightStick->GetTrigger())
robot->TankDrive(0.0, 0.0);
else
robot->TankDrive(leftStick, rightStick);
// Manipulation Joystick Code Begin
// Turret top buttons
if (manipStick->GetRawButton(4))
turret->Set(0.25);
else if (manipStick->GetRawButton(5))
turret->Set(-0.25);
// Turret controlled by x axis if top buttons arent held
else if (!manipStick->GetRawButton(5) && !manipStick->GetRawButton(4))
turret->Set(manipStick->GetX()/3);
else
turret->Set(0.0);
// Belt Controls
// button 3 held, trigger not held, moves lower belt
if (manipStick->GetRawButton(3) && !manipStick->GetTrigger())
{
upperBelt->Set(0.0);
lowerBelt->Set(-(.25));
}
// button 3 held, trigger held, moves both belts
else if (manipStick->GetRawButton(3) && manipStick->GetTrigger())
{
upperBelt->Set(-(.25));
lowerBelt->Set(-(.25));
}
// button 3 not held, trigger held, x axis moves both belts
else if (!manipStick->GetRawButton(3) && manipStick->GetTrigger())
{
upperBelt->Set(-(manipStick->GetY()));
lowerBelt->Set(-(manipStick->GetY()));
}
// button 3 not hold, trigger not held, x axis movies lower belt
else if (!manipStick->GetRawButton(3) && !manipStick->GetTrigger())
{
upperBelt->Set(0.0);
lowerBelt->Set(-(manipStick->GetY()));
}
// safety precaution, sets belts to zero if conditions above fail
else
{
lowerBelt->Set(0.0);
upperBelt->Set(0.0);
}
// Use gyro and accelerometer to get info and prints to console
printf("Accel reads: %f\n", accl->GetAcceleration());
printf("Gyro reads: %f\n", gyro->GetAngle());
Wait(0.005); // wait for a motor update time
}
}
};
START_ROBOT_CLASS(RobotMain);
|
