PDA

View Full Version : Program feedback/suggestions


csshakka
02-10-2009, 09:14 PM
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.

/*
* 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);

Jared Russell
02-11-2009, 09:19 AM
Very nice! My only comment is that you probably don't need to re-set the sensitivity and zero point of the gyro and accelerometer in the Autonomous() and OperatorControl() functions.