/******************************************************************************
* Function: DisabledPeriodic
*
* Description: Run the functions that are expected during the period when the
* robot is disabled.
******************************************************************************/
void DisabledPeriodic()
{
Scheduler::GetInstance()->Run();
Light1.Set(1);
Light2.Set(1);
while(IsDisabled())
{
UpdateActuatorCmnds(0,0,false,false,false,false,false,false,false,0,0,0,0,0);
ReadAutonSwitch();
UpdateSmartDashboad(Sw1.Get(),
Sw2.Get(),
Sw3.Get(),
Sw4.Get(),
BlSw.Get(),
(double)AutonState,
(double)0,
ballarm.GetDistance(),
gyroOne.GetAngle(),
accel.GetX(),
accel.GetY(),
accel.GetZ(),
(double)0,
(double)0);
wait(kUpdatePeriod);
}
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous()
{
dOut.Set(uint32_t(currentLedState = !currentLedState));
Wait(2.0); // for 2 seconds
dOut.Set(uint32_t(currentLedState = !currentLedState));
Wait(2.0); // for 2 seconds
}
static void* InterruptTriggerer(void* data) {
DigitalOutput* output = static_cast<DigitalOutput*>(data);
output->Set(false);
Wait(kSynchronousInterruptTime);
output->Set(true);
return nullptr;
}
virtual void SetUp() {
m_outputA = new DigitalOutput(TestBench::kLoop2OutputChannel);
m_outputB = new DigitalOutput(TestBench::kLoop1OutputChannel);
m_indexOutput = new AnalogOutput(TestBench::kAnalogOutputChannel);
m_outputA->Set(false);
m_outputB->Set(false);
m_encoder = new Encoder(TestBench::kLoop1InputChannel, TestBench::kLoop2InputChannel);
m_indexAnalogTrigger = new AnalogTrigger(TestBench::kFakeAnalogOutputChannel);
m_indexAnalogTrigger->SetLimitsVoltage(2.0, 3.0);
m_indexAnalogTriggerOutput = m_indexAnalogTrigger->CreateOutput(AnalogTriggerType::kState);
}
/**
* Output pulses to the encoder's input channels to simulate a change of 100 ticks
*/
void Simulate100QuadratureTicks() {
for(int i = 0; i < 100; i++) {
m_outputA->Set(true);
Wait(kDelayTime);
m_outputB->Set(true);
Wait(kDelayTime);
m_outputA->Set(false);
Wait(kDelayTime);
m_outputB->Set(false);
Wait(kDelayTime);
}
}
void RA14Robot::EndOfCycleMaintenance() {
//CurrentSensorReset->Set(1);
// ResetSetting = ! ResetSetting;
// CurrentSensorReset->Set(ResetSetting);
if (resetCurrentSensorTimer->Get() > Config::GetSetting(
"curent_sensor_reset_time", .1)) {
CurrentSensorReset->Set(1);
resetCurrentSensorTimer->Reset();
CurrentSensorReset->Set(0);
}
logging();
target->Parse("");
//signalOutCycle->Set(0);
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
while (IsOperatorControl() && IsEnabled())
{
Wait(2.0); // wait for a motor update time
dOut.Set(uint32_t(currentLedState = !currentLedState));
}
}
int main(void)
{
acclm.start();
delay(1000);
backlight.on();
lcd.clear();
state.setLockAccelerations(acclm);
delay(1000);
while(1)
{
istream.update(pad);
interface(istream, lcd, state, acclm);
#if DEBUG_MODE == true
lcd.print(0, 1, acclm.x - state.lockAccX);
lcd.print(4, 1, acclm.y - state.lockAccY);
lcd.print(8, 1, acclm.z - state.lockAccZ);
lcd.print(13, 1, state.safe);
#endif
if( (acclm.x-state.lockAccX>10 || acclm.x-state.lockAccX<-10 ||
acclm.y-state.lockAccY>10 || acclm.y-state.lockAccY<-10 ||
acclm.z-state.lockAccZ>10 || acclm.z-state.lockAccZ<-10)
&& state.armstate==true && state.armstate > 0)
{
state.safe--;
}
if (state.safe < SAFETY_LIMIT && state.armstate == true)
{
led.on();
buzzer.on();
}
else
{
led.off();
buzzer.off();
}
}
}
void ReadDistance() {
//// digitalWrite(trigPin, LOW); Arduino code.
// triggerPin->Set(0);
// Code to pulse. We are using pulse instead.
// distTimer->Reset(); // delay of 2 microseconds
// while(distTimer->HasPeriodPassed(0.000002) == 0) {
// ;
// }
//
//// digitalWrite(trigPin, HIGH);
// triggerPin->Set(1);
//
// distTimer->Reset(); // delay of 10 microseconds
// while(distTimer->HasPeriodPassed(0.000010) == 0) {
// ;
// }
triggerPin->Pulse(0.000010);
//// digitalWrite(trigPin, LOW); Arduino code.
// triggerPin->Set(0);
// duration = pulseIn(echoPin, HIGH); Arduino code.
duration = 0;
distTimer = 0;
while(echoPin->Get() == 0 && distTimer<1000) {
// duration = distTimer->Get();
distTimer++;
duration++;
}
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
if (distance >= maximumRange || distance <= minimumRange){
/* Send a negative number to computer to indicate "out of range" */
val = -1;
}
else {
/* Send the distance to the computer to indicate successful reading. */
val = distance;
}
SmartDashboard::PutNumber("Distance is:", val);
}
void Reset() {
m_compressor->Stop();
m_fakePressureSwitch->Set(false);
}
/**
* Set the value of a digital output.
* Set the value of a digital output to either one (true) or zero (false).
*
* @param slot The slot this digital module is plugged into
* @param channel The channel being used for this digital output
* @param value The 0/1 value set to the port.
*/
void SetDigitalOutput(UINT8 moduleNumber, UINT32 channel, UINT32 value)
{
DigitalOutput *digOut = AllocateDigitalOutput(moduleNumber, channel);
if (digOut)
digOut->Set(value);
}
void TeleopPeriodic(void) {
float x = gamepad->GetLeftX();
float y = gamepad->GetLeftY();
float rot = gamepad->GetRightX();
//small gamepad values are ignored
if (x < 0.1f && x > -0.1f)
{
x = 0;
}
if (y < 0.1f && y > -0.1f)
{
y = 0;
}
if (rot < 0.1f && rot > -0.1f)
{
rot = 0;
}
drive->MecanumDrive_Cartesian(SPEED_LIMIT * x, SPEED_LIMIT * y, SPEED_LIMIT * rot);
//shoot smoke if button is pressed
if (gamepad2->GetNumberedButton(FIRE_SMOKE_BUTTON)){
//SHOOT SMOKE!
//makingSmoke = !makingSmoke;
smoke_cannon->Set(SMOKE_CANNON_SPEED);
lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Shooting");
firing_smoke_timer->Start(); //measure how long we've fired smoke, so we know if it's ok to make more
}
else
{
smoke_cannon->Set(0.0f);
lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Not shooting");
firing_smoke_timer->Stop(); //stop the timer, since we're not firing smoke.
//don't reset, cuz we need to how much smoke we've fired.
}
//Eye Code
// float eye_pos = gamepad2->GetLeftX();
//
// right_eye_x->Set((eye_pos * 60) + default_eye_position);
// left_eye_x->Set((eye_pos * 60) + default_eye_position - LEFT_EYE_OFFSET);
//
// //button lock code
// if(gamepad2->GetNumberedButtonPressed(EYE_LOCK_BUTTON)){
// default_eye_position = eye_pos;
// }
//
//left eye control
//If A isn't pressed the value should stay the same as before
if (!gamepad2->GetNumberedButton(1)){
float left_joystick_x = gamepad2->GetLeftX();
float left_eye_x_axis = (1 - left_joystick_x)*60;
left_eye_val = left_eye_x_axis + 50;
float right_joystick_x = gamepad2->GetRawAxis(4);//right x axis
float right_eye_x_axis = (1-right_joystick_x)*60;
right_eye_val = right_eye_x_axis+20;
}
left_eye_x->SetAngle(left_eye_val);
right_eye_x->SetAngle(right_eye_val);
//float right_joystick_y = gamepad2->GetRawAxis(4);
//float right_eye_y_axis = (right_joystick_y+1)*60;
//right_eye_y->SetAngle(right_eye_y_axis);
/*
bool rbutton = gamepad2->GetNumberedButton(HEAD_UP_BUTTON);
bool lbutton = gamepad2->Ge tNumberedButton(HEAD_DOWN_BUTTON);
if (rbutton){
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "rb pressed");
jaw_motor->Set(0.2f);
}else if(lbutton){
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "lb pressed");
jaw_motor->Set(-0.15f);
}else{
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "no buttons");
jaw_motor->Set(0.0f);
}
*/
//REAL head & jaw code
//move head down
if(gamepad2->GetRightX()<=-0.5f && can_move_head_down() && can_move_jaw_down()){
head_motor->Set(-0.3f);
jaw_motor->Set(0.3f);
}
//move head up
else if(gamepad2->GetNumberedButton(HEAD_UP_BUTTON) && can_move_head_up()){
head_motor->Set(0.3f);
jaw_motor->Set(-0.3f);
}
//move jaw down
else if(gamepad2->GetRightX()>=0.5f && can_move_jaw_down()){
jaw_motor->Set(0.3f);
}
//move jaw up
else if(gamepad2->GetNumberedButton(JAW_UP_BUTTON) && can_move_jaw_up()){
jaw_motor->Set(-0.3f);
}
//sets to zero if no buttons pressed
else {
jaw_motor->Set(0.0f);
head_motor->Set(0.0f);
}
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "b:%d t:%d c:%d", bottomjaw_limit->Get(), tophead_limit->Get(), crash_limit->Get());
//Smoke code
if (gamepad2->GetNumberedButton(MAKE_SMOKE_BUTTON)){
//MAKE SMOKE!!!!!!!!!!!
//only if we don't have too much excess smoke
if (making_smoke_timer->Get() - firing_smoke_timer->Get() < MAX_EXCESS_SMOKE_TIME){
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "smoke");
smoke_machine->Set(true);
} else {
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "too much smoke");
smoke_machine->Set(false);
}
making_smoke_timer->Start(); //measure how long we've been making smoke, so we don't overflow the machine
//doesn't do anything if we've already started the timer
}
else
{
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "not smoke");
smoke_machine->Set(false);
making_smoke_timer->Stop(); //stop the timer, since we're not making smoke
//don't reset it, cuz we need to know how much smoke we've made
}
//if both timers are the same, we can set them both to zero to ensure we don't overflow them or something
if (making_smoke_timer->Get() == firing_smoke_timer->Get()){
making_smoke_timer->Reset();
firing_smoke_timer->Reset();
}
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "x:%f", x);
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "y:%f", y);
lcd->PrintfLine(DriverStationLCD::kUser_Line3, "r:%f", rot);
lcd->UpdateLCD();
}
void Reset() { m_output->Set(false); }
