mirror of
https://github.com/team2059/Zaphod
synced 2024-12-18 20:12:28 -05:00
125 lines
4.0 KiB
C++
125 lines
4.0 KiB
C++
#include "ZaphodRobot.h"
|
|
#include "ZaphodBase.h"
|
|
|
|
ZaphodRobot::ZaphodRobot():
|
|
ControlSystem(new JoystickController()),
|
|
shooter(new ZaphodShooter()),
|
|
collector(new ZaphodCollector()),
|
|
compressorSystem(new ZaphodCompressor())
|
|
{
|
|
left1 = new Jaguar(DRIVE_LEFT_SIDECAR, DRIVE_LEFT_MOTOR_ONE);
|
|
left2 = new Jaguar(DRIVE_LEFT_SIDECAR, DRIVE_LEFT_MOTOR_TWO);
|
|
left3 = new Jaguar(DRIVE_LEFT_SIDECAR, DRIVE_LEFT_MOTOR_THREE);
|
|
right1 = new Jaguar(DRIVE_RIGHT_SIDECAR, DRIVE_RIGHT_MOTOR_ONE);
|
|
right2 = new Jaguar(DRIVE_RIGHT_SIDECAR, DRIVE_RIGHT_MOTOR_ONE);
|
|
right3 = new Jaguar(DRIVE_RIGHT_SIDECAR, DRIVE_RIGHT_MOTOR_ONE);
|
|
|
|
frontSonarLeftD = new DigitalOutput(SONAR_LEFT_DIO_SIDECAR, SONAR_FRONT_LEFT_DIO);
|
|
frontSonarRightD = new DigitalOutput(SONAR_RIGHT_DIO_SIDECAR, SONAR_FRONT_RIGHT_DIO);
|
|
rearSonarLeftD = new DigitalOutput(SONAR_LEFT_DIO_SIDECAR, SONAR_REAR_LEFT_DIO);
|
|
rearSonarRightD = new DigitalOutput(SONAR_RIGHT_DIO_SIDECAR, SONAR_REAR_RIGHT_DIO);
|
|
|
|
frontSonarLeftA = new AnalogChannel(SONAR_FRONT_LEFT_ANA);
|
|
frontSonarRightA = new AnalogChannel(SONAR_FRONT_RIGHT_ANA);
|
|
rearSonarLeftA = new AnalogChannel(SONAR_REAR_LEFT_ANA);
|
|
rearSonarRightA = new AnalogChannel(SONAR_REAR_RIGHT_ANA);
|
|
}
|
|
|
|
//Functions to get sonar values and return as INCH values
|
|
|
|
float ZaphodRobot::getFrontSonar()
|
|
{
|
|
frontSonarLeftD->Set(1);
|
|
frontSonarLeftV = (frontSonarLeftA->GetAverageVoltage()/0.00488f)/2.54f;
|
|
frontSonarLeftD->Set(0);
|
|
//Probably need some sort of delay here
|
|
|
|
frontSonarRightD->Set(1);
|
|
frontSonarRightV = (frontSonarRightA->GetAverageVoltage()/0.00488f)/2.54f;
|
|
frontSonarRightD->Set(0);
|
|
|
|
//Returns the average (useful for throwing out useless readings)
|
|
return (frontSonarRightV+frontSonarLeftV)/2;
|
|
}
|
|
|
|
float ZaphodRobot::getRearSonar()
|
|
{
|
|
rearSonarLeftD->Set(1);
|
|
rearSonarLeftV = (rearSonarLeftA->GetAverageVoltage()/0.00488f)/2.54f;
|
|
rearSonarLeftD->Set(0);
|
|
//Probably need some sort of delay here
|
|
|
|
rearSonarRightD->Set(1);
|
|
rearSonarRightV = (rearSonarRightA->GetAverageVoltage()/0.00488f)/2.54f;
|
|
rearSonarRightD->Set(0);
|
|
|
|
//Returns the average (useful for throwing out useless readings)
|
|
return (rearSonarRightV+rearSonarLeftV)/2;
|
|
}
|
|
|
|
bool ZaphodRobot::checkJoystickValues()
|
|
{
|
|
float x = ControlSystem->rightJoystickAxisValues[1];
|
|
float y = ControlSystem->rightJoystickAxisValues[2];
|
|
if((-.1 < x && x < .1) && (-.1 < y && y < .1))
|
|
{
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void ZaphodRobot::driveRobot(float x, float y)
|
|
{
|
|
if(y>1.0f){
|
|
y=1.0f;
|
|
}else if(y!=0.0f&&y<-1.0f){
|
|
y=-1.0f;
|
|
}
|
|
float leftPower=((y+x)/2+1)*127+1;
|
|
float rightPower=((y-x)/2+1)*127+1;
|
|
right1->SetRaw(int(rightPower));
|
|
right2->SetRaw(int(rightPower));
|
|
right3->SetRaw(int(rightPower));
|
|
left1->SetRaw(int(leftPower));
|
|
left2->SetRaw(int(leftPower));
|
|
left3->SetRaw(int(leftPower));
|
|
}
|
|
|
|
//Main function used to handle periodic tasks on the robot
|
|
|
|
void ZaphodRobot::handler()
|
|
{
|
|
//Periodic tasks that should be run by every loop
|
|
ControlSystem->updateJoysticks();
|
|
shooter->updateShooterPosition();
|
|
//TODO Need to implement a timing system to not break the spike (this function doesn't run the compressor at the moment)
|
|
compressorSystem->compressorSystemPeriodic();
|
|
collector->updateCollector(shooter->isShooting, shooter->getAngle());
|
|
driveRobot(ControlSystem->rightJoystickAxisValues[3]+ControlSystem->rightJoystickAxisValues[1], -ControlSystem->rightJoystickAxisValues[2]);
|
|
|
|
//Button assignments to actions
|
|
if(ControlSystem->leftJoystickValues[SHOOTER_FIRE])
|
|
{
|
|
shooter->startShootingSequence(ControlSystem->throttle);
|
|
}
|
|
if(ControlSystem->rightJoystickValues[COLLECTOR_INTAKE])
|
|
{
|
|
collector->collectBall();
|
|
}
|
|
if(ControlSystem->rightJoystickValues[COLLECTOR_OUTTAKE])
|
|
{
|
|
collector->releaseBall();
|
|
}
|
|
if(ControlSystem->rightJoystickValues[COLLECTOR_EXTEND])
|
|
{
|
|
compressorSystem->extendCollector();
|
|
}
|
|
if(ControlSystem->rightJoystickValues[COLLECTOR_RETRACT])
|
|
{
|
|
compressorSystem->retractCollector();
|
|
}
|
|
}
|