/******************************************************************************
 * 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);
  }
}
Exemple #2
0
	/**
	 * 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;
}
Exemple #4
0
	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);
	}
Exemple #5
0
	/**
	 * 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);
		}
	}
Exemple #6
0
	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);
	}
Exemple #7
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);
	}
Exemple #10
0
 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); }