void Reset() {
m_talonCounter->Reset();
m_victorCounter->Reset();
m_jaguarCounter->Reset();
m_talon->Set(0.0f);
m_victor->Set(0.0f);
m_jaguar->Set(0.0f);
}
void TeleopPeriodic()
{
char myString [STAT_STR_LEN];
if (initialZeroing) // moving to the inner limit
{
sprintf(myString, "initZero ip\n"); //in progress
SmartDashboard::PutString("DB/String 0", myString);
if (GetForkLimitSwitchMin())
{
SetForkMotor(MOTOR_SPEED_STOP);
gearToothCounter->Reset();
initialZeroing = false;
sprintf(myString, "initZero comp\n"); //complete
SmartDashboard::PutString("DB/String 0", myString);
}
}
else //manual control
{
//motor control
if (joystick->GetRawButton(BUT_JS_OUT) && !GetForkLimitSwitchMax()) // move outwards
SetForkMotor(MOTOR_SPEED_GO);
else if(joystick->GetRawButton(BUT_JS_IN) && !GetForkLimitSwitchMin()) // move inwards
SetForkMotor(-MOTOR_SPEED_GO);
else
SetForkMotor(MOTOR_SPEED_STOP); //stop
//counter control
if (joystick->GetRawButton(BUT_JS_RES_GTC)) // reset the gear tooth counter
gearToothCounter->Reset();
}
//status
sprintf(myString, "jsOut %d\n", joystick->GetRawButton(BUT_JS_OUT));
SmartDashboard::PutString("DB/String 1", myString);
sprintf(myString, "jsIn %d\n", joystick->GetRawButton(BUT_JS_IN));
SmartDashboard::PutString("DB/String 2", myString);
sprintf(myString, "jsResGtc %d\n", joystick->GetRawButton(BUT_JS_RES_GTC));
SmartDashboard::PutString("DB/String 3", myString);
sprintf(myString, "curr: %f\n", forkMotor->GetOutputCurrent());
SmartDashboard::PutString("DB/String 4", myString);
sprintf(myString, "gtc #: %d\n", gearToothCounter->Get()); //gtc count
SmartDashboard::PutString("DB/String 5", myString);
}
// Update normal light sequence by checking the main counter
int UpdateNormalSequence(int goLightTime) {
int i = 1;
while (mainCounter.GetCounter() > i * goLightTime)
i++;
// On Select Sequence Button, cycle through the sequences immediately
if (btnSelectSequence.IsPressed()) {
// reset the main counter to start of new sequence
mainCounter.Reset(i * goLightTime);
i++;
}
// if maximum sequences has reached reset the main counter
if (i == MAX_SEQUENCES + 1) {
mainCounter.Reset();
i = 1;
}
return i;
}
void setup()
{
Serial.begin(SERIAL_BAUD);
trafficLight0.SetPins(
PIN_GREEN_LIGHT_0,
PIN_ORANGE_LIGHT_0,
PIN_RED_LIGHT_0,
PIN_FILTERTURN_LIGHT_0,
PIN_PED_LIGHT_0,
PIN_PEDCROSS_BUZZER_0
);
trafficLight90.SetPins(
PIN_GREEN_LIGHT_90,
PIN_ORANGE_LIGHT_90,
PIN_RED_LIGHT_90,
PIN_FILTERTURN_LIGHT_90,
PIN_PED_LIGHT_90,
PIN_PEDCROSS_BUZZER_90
);
trafficLight270.SetPins(
PIN_GREEN_LIGHT_270,
PIN_ORANGE_LIGHT_270,
PIN_RED_LIGHT_270,
PIN_FILTERTURN_LIGHT_270,
PIN_PED_LIGHT_270,
PIN_PEDCROSS_BUZZER_270
);
mainCounter.Reset();
pot.SetPin(PIN_POT);
pot.ApplySmoothing();
ther.SetPin(PIN_THER);
ther.SetFixedResistorR2(10000);
ther.SetVIn(5);
ther.ApplySmoothing();
ldr.SetPin(PIN_LDR);
ldr.SetFixedResistorR2(10000);
ldr.SetVIn(5);
ldr.ApplySmoothing();
btnEmergency.SetPin(PIN_BTN_EMERGENCY);
btnRoadWork.SetPin(PIN_BTN_ROADWORK);
btnSelectSequence.SetPin(PIN_BTN_SELECTSEQUENCE);
btnPedCross0.SetPin(PIN_BTN_PEDCROSS_0);
btnPedCross270.SetPin(PIN_BTN_PEDCROSS_270);
btnPedCross90.SetPin(PIN_BTN_PEDCROSS_90);
}
// on road work mode flash orange light only at all junction
void FlashForRoadWork() {
static Counter roadWorkCounter;
// flash the orange light by checking the counter time
if(roadWorkCounter.GetCounter() <= 500) {
trafficLight0.RoadWorkLightOnly();
trafficLight90.RoadWorkLightOnly();
trafficLight270.RoadWorkLightOnly();
}
else if(roadWorkCounter.GetCounter() <= 1000) {
trafficLight0.ledOrange.SwitchOff();
trafficLight90.ledOrange.SwitchOff();
trafficLight270.ledOrange.SwitchOff();
}
else
roadWorkCounter.Reset();
}
// on ped crossing mode when time is finished flash ped light at all junction
void FlashForPedCrossFinish() {
static Counter pedCrossCounter;
// flash the ped light by checking the counter time
if(pedCrossCounter.GetCounter() <= 500) {
trafficLight270.ledPed.SwitchOn();
trafficLight0.ledPed.SwitchOn();
trafficLight90.ledPed.SwitchOn();
}
else if(pedCrossCounter.GetCounter() <= 1000) {
trafficLight0.ledPed.SwitchOff();
trafficLight90.ledPed.SwitchOff();
trafficLight270.ledPed.SwitchOff();
}
else
pedCrossCounter.Reset();
}
// this function simulates normal light condition when called at every loop
// it automatically transitions from green to red signal by using the OrangeTransitionCounter
void UpdateWithLightTransition(bool goSignal, bool turnLight, bool pedLight, int orangeLightTime) {
// if green or go signal turn off transitioning process
// and only light up the green light
if (goSignal) {
transitionState = 0;
GreenLight();
}
// if red signal and green light was previously on
// start transitioning to red by firt turning on the
// orange light
else if(!goSignal && ledGreen.IsOn() && transitionState == 0) {
OrangeLight();
transitionState = 1;
OrangeTransitionCounter.Reset();
}
// if red signal and greenlight was not on, directly
// switch on the red light
else if(!goSignal && !ledGreen.IsOn() && transitionState == 0)
RedLight();
// check if we are still transitioning to red light
// if orangeTransitionCounter has passed orangeLightTime interval
// turn on the Red light only
if (transitionState == 1) {
if (OrangeTransitionCounter.GetCounter() < orangeLightTime)
OrangeLight();
else
transitionState = 2;
}
if (transitionState == 2)
RedLight();
if (turnLight)
ledFilterTurn.SwitchOn();
else
ledFilterTurn.SwitchOff();
// ped lights turn on only when the normal lights are in red signal
if (pedLight && ledRed.IsOn())
ledPed.SwitchOn();
else
ledPed.SwitchOff();
}
int main()
{
Counter MomCounter;
char choice;
cout<<setw(4)<<setfill('0') << endl <<MomCounter.GetCount() << endl << endl;
cin>>choice;
choice=toupper(choice);
while(choice!='X')
{
switch(choice)
{
case 'F':
if(MomCounter.GetCount()+1<=MomCounter.GetMax())
{
MomCounter.Incr1();
}
else
{
cout << "\n|OVERFLOW|\n";
}
break;
case 'D':
if(MomCounter.GetCount()+10<=MomCounter.GetMax())
{
MomCounter.Incr10();
}
else
{
cout << "\n|OVERFLOW|\n";
}
break;
case 'S':
if(MomCounter.GetCount()+100<=MomCounter.GetMax())
{
MomCounter.Incr100();
}
else
{
cout << "\n|OVERFLOW|\n";
}
break;
case 'A':
if(MomCounter.GetCount()+1000<=MomCounter.GetMax())
{
MomCounter.Incr1000();
}
else
{
cout << "\n|OVERFLOW|\n";
}
break;
case 'R':
MomCounter.Reset();
break;
}
cout<<setw(4)<<setfill('0') << endl <<MomCounter.GetCount() << endl;
cout<<"\n:" ;
cin>>choice;
choice = toupper(choice);
}
return 0;
}
/**
* Reset the Counter to zero.
* Set the counter value to zero. This doesn't effect the running state of the counter, just sets
* the current value to zero.
* @param slot The slot the digital module is plugged into
* @param channel The channel of the digital input used with this counter
*/
void ResetCounter(UINT8 moduleNumber, UINT32 channel)
{
Counter *counter = AllocateCounter(moduleNumber, channel);
if (counter != NULL)
counter->Reset();
}
/**
* Reset the Counter to zero.
* Set the counter value to zero. This doesn't effect the running state of the counter, just sets
* the current value to zero.
* @param channel The channel of the digital input used with this counter
*/
void ResetCounter(UINT32 channel)
{
Counter *counter = AllocateCounter(channel);
if (counter != NULL)
counter->Reset();
}
// function to allow for ped crossing on the press of a button
// ped crossing works by pausing the main counter at the end of a sequence
// and resuming after the ped has crossed.
// returns whether alarm is currently on progression or not
bool AddPedCrossStageOnRequest(int state, int pedCrossTime, int orangeLightTime){
#define NO_PED_CROSS 0
#define PED_CROSS_BTN_PRESSED 1
#define PED_CROSS_IN_PROGRESS 2
static int alarmState = 0;
// check for button presses at all the junctions
if(btnPedCross0.IsPressed() || btnPedCross90.IsPressed() || btnPedCross270.IsPressed())
if (alarmState == NO_PED_CROSS) alarmState = PED_CROSS_BTN_PRESSED;
// update the buzzer asyncronously
trafficLight0.crossingBuzzer.UpdateAsync();
trafficLight90.crossingBuzzer.UpdateAsync();
trafficLight270.crossingBuzzer.UpdateAsync();
static int prevState = 0;
static Counter pedCrossCounter;
// check if the current sequence has ended
if (prevState != state && alarmState == PED_CROSS_BTN_PRESSED) {
// pause the timer
mainCounter.Pause();
// reset the crossing counter to 0
pedCrossCounter.Reset();
// activate the flag to transition all the green signal to red first
alarmState = PED_CROSS_IN_PROGRESS;
return true;
}
if (alarmState == PED_CROSS_IN_PROGRESS) {
if (pedCrossCounter.GetCounter() <= pedCrossTime) {
// if there were no green lights from the previous sequence, directly turn on the red light
if (trafficLight0.ledRed.IsOn() && trafficLight90.ledRed.IsOn() && trafficLight270.ledRed.IsOn())
{
trafficLight270.UpdateWithLightTransition(0, 0, 1, orangeLightTime);
trafficLight0.UpdateWithLightTransition(0, 0, 1, orangeLightTime);
trafficLight90.UpdateWithLightTransition(0, 0, 1, orangeLightTime);
trafficLight0.crossingBuzzer.TriggerAlarm();
trafficLight90.crossingBuzzer.TriggerAlarm();
trafficLight270.crossingBuzzer.TriggerAlarm();
}
// if there were green lights on the previous sequence, transition to red first
else {
trafficLight270.UpdateWithLightTransition(0, 0, 0, orangeLightTime);
trafficLight0.UpdateWithLightTransition(0, 0, 0, orangeLightTime);
trafficLight90.UpdateWithLightTransition(0, 0, 0, orangeLightTime);
trafficLight0.crossingBuzzer.TurnOffAlarm();
trafficLight90.crossingBuzzer.TurnOffAlarm();
trafficLight270.crossingBuzzer.TurnOffAlarm();
}
return true;
}
// if ped cross time has finish blink the ped light as in australian ped crossing
else if (pedCrossCounter.GetCounter() <= pedCrossTime + orangeLightTime) {
FlashForPedCrossFinish();
trafficLight0.crossingBuzzer.TurnOffAlarm();
trafficLight90.crossingBuzzer.TurnOffAlarm();
trafficLight270.crossingBuzzer.TurnOffAlarm();
return true;
}
else {
alarmState = NO_PED_CROSS;
}
}
prevState = state;
return false;
}
TRAFFICLIGHT() {
OrangeTransitionCounter.Reset();
transitionState = 0;
}