Program feedback/suggestions

[csshakka]

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]

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.