int RelayHandler::ChangeRelayState(int relayNum, bool turnOn){
Relay *r = findRelayByAddress(relayNum);
if(r == NULL){
return -1;
}
long now = (long)millis();
if(turnOn){
long lastOn = r->LastOn();
Serial.print(lastOn);
Serial.print(", ");
Serial.print(CycleOffTimeMS);
Serial.print(", ");
Serial.println(now);
if(lastOn != 0 && (lastOn + CycleOffTimeMS > now)){
Serial.print(r->Address());
Serial.println(": cannot be turned on due to minimum times");
return -1; //not enough time has ellapsed
}else{
//minimum time ellapsed since on
r->TurnOn();
Serial.print(relayNum);
Serial.println(": Relay turned on");
}
}else{
r->TurnOff();
Serial.print(relayNum);
Serial.println(": Relay turned off");
}
}
LateReturn<lo::Message> SCLang::SendOSCCustomWithLOReply(const std::string& path, const lo::Message &m){
Relay<lo::Message> r;
if(!Config::Global().use_sc) return r;
if(!osc) {std::cout << "WARNING: Failed to send OSC message to server, OSC not ready" << std::endl; return r;}// throw Exceptions::SCLangException("Failed to send OSC message to server, OSC not yet ready");
osc->Send(path, [=](lo::Message msg){
r.Return(msg);
}, m);
return r;
}
LateReturn<std::shared_ptr<Module>> Canvas::CreateModule(std::string id){
Relay<std::shared_ptr<Module>> r;
ModuleFactory::CreateNewInstance(id, shared_from_this()).Then([this,r](std::shared_ptr<Module> m){
modules.emplace(m);
m->canvas = shared_from_this();
r.Return(m);
}).Catch(r);
return r;
}
//
// Main Tele Operator Mode function. This function is called once, therefore a while loop that checks IsOperatorControl and IsEnabled is used
// to maintain control until the end of tele operator mode
//
void OperatorControl()
{
//myRobot.SetSafetyEnabled(true);
timer->Start();
Relay* reddlight = new Relay(4);
//Timer* lighttimer = new Timer();
//lighttimer->Start();
while (IsOperatorControl() && IsEnabled())
{
reddlight->Set(reddlight->kForward);
/*
if (lighttimer->Get()<=0.5) {
reddlight->Set(reddlight->kForward);
}
else if(lighttimer->Get()<1){
reddlight->Set(reddlight->kOff);
}
else {
lighttimer->Reset();
}
*/
//
// Get inputs
//
driverInput->GetInputs();
drive->GetInputs();
catapult->GetInputs();
feeder->GetInputs();
//
// Pass values between components as necessary
//
//catapult->SetSafeToFire(feeder->GetAngle()<95);
//
// Execute one step on each component
//
drive->ExecStep();
catapult->ExecStep();
feeder->ExecStep();
//
// Set Outputs on all components
//
catapult->SetOutputs();
feeder->SetOutputs();
//
// Wait for step timer to expire. This allows us to control the amount of time each step takes. Afterwards, restart the
// timer for the next loop
//
while (timer->Get()<(PERIOD_IN_SECONDS));
timer->Reset();
}
}
LateReturn<> SCLang::InstallTemplate(const std::shared_ptr<ModuleTemplate> t){
Relay<> r;
if(!t->has_sc_code) return r.Return();
SendOSCWithEmptyReply("/algaudioSC/installtemplate", "ss", t->GetFullID().c_str(), t->sc_code.c_str()).Then([=](){
installed_templates.insert(t->GetFullID());
std::cout << "Template " << t->GetFullID() << " installed." << std::endl;
r.Return();
});
return r;
}
void LEDLights (bool onoff) //light function
{
if (onoff) {
rlyLED->Set(Relay::kForward);// turn on LEDs
} else {
rlyLED->Set(Relay::kOff);// turn off LEDs
}
}
SebastianRobot(void) {
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Sebastian V24.2");
dsLCD->UpdateLCD();
GetWatchdog().SetEnabled(false);
led0 = new Relay(2);
led1 = new Relay(3);
flashCount = 0;
led0->Set(Relay::kOff);
led1->Set(Relay::kOff);
isFlashing=false;
}
void toggleBrake()
{
if(isBraking())
{
m_brake->Set(m_brake->kForward);
m_bIsBraking = false;
}
else
{
m_brake->Set(m_brake->kReverse);
m_bIsBraking = true;
}
}
LateReturn<lo::Message> SCLang::SendOSCWithLOReply(const std::string &path, std::string tag, ...){
Relay<lo::Message> r;
if(!Config::Global().use_sc) return r;
if(!osc) {std::cout << "WARNING: Failed to send OSC message to server, OSC not ready" << std::endl; return r;} // throw Exceptions::SCLangException("Failed to send OSC message to server, OSC not yet ready");
va_list q;
va_start(q, tag);
lo::Message m;
std::string t = tag + "$$";
m.add_varargs(t, q);
osc->Send(path, [=](lo::Message msg){
r.Return(msg);
}, m);
return r;
}
/**
* Set the relay state.
*
* Valid values depend on which directions of the relay are controlled by the object.
*
* When set to kBothDirections, the relay can only be one of the three reasonable
* values, 0v-0v, 0v-12v, or 12v-0v.
*
* When set to kForwardOnly or kReverseOnly, you can specify the constant for the
* direction or you can simply specify kOff and kOn. Using only kOff and kOn is
* recommended.
*
* @param slot The slot that the digital module is plugged into
* @param channel The relay channel number for this object
* @param value The state to set the relay.
*/
void SetRelay(UINT32 slot, UINT32 channel, RelayValue value)
{
Relay *relay = AllocateRelay(slot, channel);
if (relay != NULL)
{
switch (value)
{
case kOff: relay->Set(Relay::kOff); break;
case kOn: relay->Set(Relay::kOn); break;
case kForward: relay->Set(Relay::kForward); break;
case kReverse: relay->Set(Relay::kReverse); break;
}
}
}
LateReturn<> Subpatch::on_init_latereturn(){
Relay<> r;
Sync s(5);
auto parent = canvas.lock();
inlets.resize(4);
bool fake = ! Config::Global().use_sc;
Module::Inlet::Create("in1","Inlet 1",shared_from_this(),fake).Then([this,s](auto inlet){
inlets[0] = inlet;
if(entrance) entrance->LinkOutput(0, inlet->bus->GetID());
s.Trigger();
});
Module::Inlet::Create("in2","Inlet 2",shared_from_this(),fake).Then([this,s](auto inlet){
inlets[1] = inlet;
if(entrance) entrance->LinkOutput(1, inlet->bus->GetID());
s.Trigger();
});
Module::Inlet::Create("in3","Inlet 3",shared_from_this(),fake).Then([this,s](auto inlet){
inlets[2] = inlet;
if(entrance) entrance->LinkOutput(2, inlet->bus->GetID());
s.Trigger();
});
Module::Inlet::Create("in4","Inlet 4",shared_from_this(),fake).Then([this,s](auto inlet){
inlets[3] = inlet;
if(entrance) entrance->LinkOutput(3, inlet->bus->GetID());
s.Trigger();
});
s.WhenAll([r, this](){
if(modulegui) modulegui->OnInletsChanged();
r.Return();
});
Canvas::CreateEmpty(parent).Then([this,s](auto c){
c->owner_hint = this->shared_from_this();
internal_canvas = c;
Sync s2(2);
c->CreateModule("builtin/subentry").Then([this,s2](auto module){
module->position_in_canvas = Point2D(0,-300);
s2.Trigger();
});
c->CreateModule("builtin/subexit").Then([this,s2](auto module){
module->position_in_canvas = Point2D(0,300);
s2.Trigger();
});
s2.WhenAll([s](){s.Trigger();});
});
return r;
}
void OperatorControl(void) {
XboxController *xbox = XboxController::getInstance();
bool isEndGame = false;
GetWatchdog().SetEnabled(true);
_driveControl.initialize();
//_poleVaultControl.initialize();
shooterControl.InitializeOperator();
while (IsEnabled() && IsOperatorControl()) {
GetWatchdog().Feed();
dsLCD->Clear();
if (xbox->isEndGame()) {
isEndGame = !isEndGame;
}
if (!isEndGame) {
shooterControl.Run();
//_poleVaultControl.act();
_driveControl.act();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Normal");
led0->Set((shooterControl.getLED1())?Relay::kOn: Relay::kOff);
led1->Set((shooterControl.getLED2())?Relay::kOn: Relay::kOff);
}
else {
shooterControl.RunEndGame();
//_poleVaultControl.actEndGame();
_driveControl.actEndGame();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "End Game");
flashCount--;
if (flashCount<=0){
isFlashing = !isFlashing;
flashCount=FLASHTIME;
}
led0->Set((isFlashing)?Relay::kOn: Relay::kOff);
led1->Set((isFlashing)?Relay::kOn: Relay::kOff);
}
// dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "Flash: %i", flashCount);
dsLCD->UpdateLCD();
Wait(WAIT_TIME); // wait for a motor update time
}
GetWatchdog().SetEnabled(false);
}
void DisabledInit() {
//Config loading
try {
cameraLight->Set(Relay::kOff);
if (!Config::LoadFromFile("config.txt")) {
cout << "Something happened during the load." << endl;
}
Config::Dump();
myDrive->DisablePID();
myDrive->ResetPID();
if(fout.is_open() && !freshStart && !ds->IsFMSAttached()){
fout.close();
myShooter->ResetShooterProcess();
lc->holdState(false);
}
} catch (exception ex) {
cout << "Disabled exception. Trying again." << endl;
cout << "Exception: " << ex.what() << endl;
}
//ResetShooterMotors();
/*
SmartDashboard::PutNumber("Target Info S",1741);
cout<<SmartDashboard::GetNumber("Target Info S");
*/
}
RobotDemo()
{
//Initialize the objects for the drive train
myRobot = new RobotDrive(1, 2);
leftEnco = new Encoder(LEFT_ENCO_PORT_1, LEFT_ENCO_PORT_2);
rightEnco = new Encoder(RIGHT_ENCO_PORT_1, RIGHT_ENCO_PORT_2);
leftEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
leftEnco->Start();
rightEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
rightEnco->Start();
leftStick = new Joystick(1);
rightStick = new Joystick(2);
//Initialize the gyro
gyro = new Gyro(GYRO_PORT);
gyro->Reset();
//Initialize the manipulators
intake = new Intake(INTAKE_ROLLER_PORT, BALL_SENSOR_PORT, LEFT_SERVO_PORT, RIGHT_SERVO_PORT);
catapult = new Catapult(LOADING_MOTOR_PORT, HOLDING_MOTOR_PORT, LOADED_LIMIT_1_PORT,
LOADED_LIMIT_2_PORT, HOLDING_LIMIT_PORT);
//Initialize the objects needed for camera tracking
rpi = new RaspberryPi("17140");
LEDLight = new Relay(1);
LEDLight->Set(Relay::kForward);
//Set the autonomous modes
autonMode = ONE_BALL_AUTON;
autonStep = AUTON_ONE_SHOOT;
//Initialize the lcd
lcd = DriverStationLCD::GetInstance();
}
/**
* Initialization code for autonomous mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters autonomous mode.
*/
void RA14Robot::AutonomousInit() {
Config::LoadFromFile("config.txt");
auto_case = (int) Config::GetSetting("auto_case", 1);
alreadyInitialized = true;
auto_timer->Reset();
auto_timer->Start();
missionTimer->Start();
myDrive->ResetOdometer();
myCamera->Set(Relay::kForward);
myCollection->ExtendArm();
cout<<"Reseting Gyro"<<endl;
gyro->Reset();
//myOdometer->Reset();
//myDrive->ShiftUp();
myDrive->ShiftDown();
//shift to high gear
if (!fout.is_open()) {
cout << "Opening logging.csv..." << endl;
fout.open("logging.csv");
logheaders();
}
auto_state = 0;
#ifndef DISABLE_SHOOTER
myCam->Reset();
myCam->Enable();
#endif //Ends DISABLE_SHOOTER
}
LateReturn<> SubpatchExit::on_init_latereturn(){
Relay<> r;
Sync s(4);
// Check if parent canvas is managed by a subpatch.
std::shared_ptr<Module> owner = canvas.lock()->owner_hint;
auto owner_subpatch = std::dynamic_pointer_cast<Subpatch>(owner);
// If not, throw a DoNotWantToBeCreated exception.
if(!owner_subpatch) {r.LateThrow<Exceptions::ModuleDoesNotWantToBeCreated>("This module can only be created inside a subpatch."); return r;}
if(owner_subpatch->HasExit()) {r.LateThrow<Exceptions::ModuleDoesNotWantToBeCreated>("Currently it is not possible to create multiple exits inside the same subpatch."); return r;}
// Otherwise link to that entrance.
owner_subpatch->LinkToExit(std::static_pointer_cast<SubpatchExit>( shared_from_this() ));
subpatch = owner_subpatch;
// Create inlets.
inlets.resize(4);
bool fake = ! Config::Global().use_sc;
Module::Inlet::Create("in1","Inlet 1",shared_from_this(),fake).Then([this,s](auto inlet){
inlets[0] = inlet;
subpatch->LinkOutput(0, inlet->bus->GetID());
s.Trigger();
});
Module::Inlet::Create("in2","Inlet 2",shared_from_this(),fake).Then([this,s](auto inlet){
inlets[1] = inlet;
subpatch->LinkOutput(1, inlet->bus->GetID());
s.Trigger();
});
Module::Inlet::Create("in3","Inlet 3",shared_from_this(),fake).Then([this,s](auto inlet){
inlets[2] = inlet;
subpatch->LinkOutput(2, inlet->bus->GetID());
s.Trigger();
});
Module::Inlet::Create("in4","Inlet 4",shared_from_this(),fake).Then([this,s](auto inlet){
inlets[3] = inlet;
subpatch->LinkOutput(3, inlet->bus->GetID());
s.Trigger();
});
s.WhenAll([r, this](){
if(modulegui) modulegui->OnInletsChanged();
r.Return();
});
return r;
}
void AutonomousInit() {
init();
currentDistance = 0;
goalDistance = 6.0*12.0; // 6 feet
autonomousShooterDelay = 0;
shooterMotor->Set(0);
loaderMotor->Set(Relay::kOff);
}
/**
* Initialization code for test mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters test mode.
*/
void RA14Robot::TestInit() {
myCam->Disable();
myCompressor->Start();
Config::LoadFromFile("config.txt");
Config::Dump();
myCamera->Set(Relay::kForward); // turn on light
}
void relay_time_count( void) {
relay.time_current += RELAY_TIMER_RESOLUTION;
if( relay.time_current>=relay.time_target) {
relay.stop();
//relay.callback_finish();
} else {
//relay.callback_update();
}
}
C1983_Lift(Encoder *liftSensorChannel, Relay *brake)
{
m_liftSensor = liftSensorChannel;
m_brake = brake;
m_bIsBraking = true;
m_brake->SetDirection(m_brake->kBothDirections);
m_liftSensor->SetDistancePerPulse((float)LIFTDISTPERPULSE);
}
virtual void AutonomousInit() {
shooterAngle->Set(Relay::kForward);
rightDrive->SetSpeed(0);
leftDrive->SetSpeed(0);
shooterFWD->SetSpeed(1);
shooterRear->SetSpeed(1);
sleep(4);
cout << "BEGIN" << endl;
shooterFire->Set(Relay::kForward);
sleep(1);
shooterFire->Set(Relay::kReverse);
sleep(1);
//nanosleep(6);
shooterFire->Set(Relay::kForward);
sleep(1);
shooterFire->Set(Relay::kReverse);
sleep(1);
shooterFire->Set(Relay::kForward);
sleep(1);
shooterFire->Set(Relay::kReverse);
sleep(1);
shooterFire->Set(Relay::kForward);
sleep(1);
shooterFire->Set(Relay::kReverse);
sleep(2);
shooterFWD->SetSpeed(0);
shooterRear->SetSpeed(0);
sleep(2);
shooterAngle->Set(Relay::kReverse);
cout << "END" << endl;
}
void Test()
{
DriverStationLCD *screen = DriverStationLCD::GetInstance();
while (IsTest())
{
if ((buttonOne.Get()) == 1)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Not Pressed!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Pressed!");
}
if ((buttonTwo.Get()) == 1)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Not Pressed!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Pressed!");
}
if ((togglebuttonOne.Get()) == 0)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Is_On!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Is_Off!");
}
if ((togglebuttonTwo.Get()) == 0)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Is_On!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Is_Off!");
}
if (buttonOne.Get() == 0)
{
emergencyCompressor.Set(Relay::kOff);
}
#if 0
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"ButtonOneIs_%d", buttonOne.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"toggle_%d", buttonTwo.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Button_%d", togglebuttonOne.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"toggle_%d", togglebuttonTwo.Get());
#endif
Wait(0.005);
screen -> UpdateLCD();
}
}
LateReturn<std::shared_ptr<Canvas>> Canvas::CreateEmpty(std::shared_ptr<Canvas> parent){
Relay<std::shared_ptr<Canvas>> r;
auto res = std::shared_ptr<Canvas>( new Canvas() );
res->parent = parent;
auto parentgroup = (parent)? parent->GetGroup() : nullptr;
if(Config::Global().use_sc){
Group::CreateNew( parentgroup ).Then([r,res](std::shared_ptr<Group> g){
res->group = g;
std::cout << "New canvas group " << g->GetID() << std::endl;
r.Return(res);
});
}else{
std::cout << "Note: Using a FAKE group to create a new empty canvas" << std::endl;
Group::CreateFake( parentgroup ).Then([r,res](std::shared_ptr<Group> g){
res->group = g;
r.Return(res);
});
}
return r;
}
void Disabled()
{
while(IsDisabled())
{
LEDLight->Set(Relay::kForward);
rpi->Read();
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "R: %i", rpi->GetMissingPacketcount());
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "x: %i", rpi->GetXPos());
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "y: %i", rpi->GetYPos());
lcd->UpdateLCD();
}
}
bool BusVoltageCheck(void)
{
bool Fail = false;
// Die Überwachung auf Busspannungsausfall ist zweigeteilt:
// - Unterschreitung einer absoluten Mindestspannung auf dem Bus
// - Absinken der Busspannung unter ein Level, wo ein Halten der Mindestreserve für Schaltaktionen bei
// Busspannungsausfall nicht mehr sichergestellt ist.
if (GetBusVoltage() < (int)MINUBUSVOLTAGEFALLING)
Fail = true;
// Im Disable-Zustand oder während MeasMode liefert BusVoltageFailRailLevel() keine sinnvollen Ergebnisse,
// da die Energiemenge noch unbekannt, die für einen Schaltvorgang notwendig ist.
if (relay.IsOperating())
{
if (relay.BusVoltageFailRailLevel())
{
Fail = true;
}
}
if (Fail)
{
relay.BusVoltageFailed();
}
return not Fail;
}
void ControlDeviceChanel(int relayIndex, int chanel, int switchStatus)
{
//std::cout << "in ControlDeviceChanel: port chanel status:" << relayIndex << chanel << switchStatus << std::endl;
//获取串口对象
std::map<int, Relay*>::iterator iter = g_mapPortRelay.find(relayIndex);
if (iter != g_mapPortRelay.end())
{
Relay *pRelay = iter->second;
pRelay->SetBoardChanel(chanel, switchStatus);
}
else//没有创建继电器对象，则创建一个继电器对象
{
Relay *pRelay = new Relay;
int ret = pRelay->InitRelay(relayIndex);
if(ret == -1)
{
return;
}
std::pair<int, Relay*>serialRelay(relayIndex, pRelay);
g_mapPortRelay.insert(serialRelay);
pRelay->SetBoardChanel(chanel, switchStatus);
}
}
void TeleopInit() {
//Open logging
if(!fout.is_open()) {
fout.open("logging.csv");
logheaders();
}
cameraLight->Set(Relay::kOff);
myDrive->SetPID(KP,KI,KD);
myDrive->EnablePID();
manualAngle = 0;
compressor->Start();
myDrive->setGyroDrive(true);
lc->setMode(0);
freshStart = false;
}
void OperatorControl(void)
{
myRobot->SetSafetyEnabled(false);
LEDLights (true); //turn camera lights on
shooterspeedTask->Start((UINT32)this); //start counting shooter speed
kickerTask->Start((UINT32)this); //turns on the kicker task
kicker_in_motion = false;
while (IsOperatorControl() && !IsDisabled())
{
if (ControllBox->GetDigital(3)) //turn tracking on with switch 3 on controll box
{
tracking(ControllBox->GetDigital(7));
}
else
{
myRobot->TankDrive(leftstick, rightstick); //if tracking is off, enable human drivers
Wait(0.005); // wait for a motor update time
}
Shooter_onoff=ControllBox->GetDigital(4); //shoot if switch 4 is on
ballgatherer(ControllBox->GetDigital(5), rightstick->GetRawButton(10));
kicker_onoff=lonelystick->GetRawButton(1);
bridgeboot(ControllBox->GetDigital(6));
kicker_cancel=lonelystick->GetRawButton(2);
//kicker_down=rightstick->GetRawButton(11));
}
LEDLights (false);
shooterspeedTask->Stop();
kickerTask->Stop();
ballgatherer(false, false);
kickermotor->Set(Relay::kOff);
}
void AutonomousPeriodic() {
currentDistance = leftDriveEncoder->GetDistance();
shooterMotor->Set(-shooterMotorVolts);
if (currentDistance >= goalDistance){
drive->setLeft(0);
drive->setRight(0);
loaderMotor->Set(Relay::kForward);
} else {
drive->setLeft(.49);
drive->setRight(.5);
}
//log->info("LRD %f %f",
// leftDriveEncoder->GetDistance(),
// rightDriveEncoder->GetDistance());
//log->print();
}
RobotDemo(void)
{
///constructs
game = false;//set to true or false before use- true for auton & tele-op, false for just 1
cd = new CustomDrive(NUM_JAGS);
closed = new Solenoid(8, 1);//true if closed
open = new Solenoid(8, 2);
stick = new Joystick(2);// arm controll
controller = new Joystick(1);// base
manEnc = new Encoder(4,5);
reset = false;//if button
manJag = new Jaguar(6);//cons manip Jag
ShoulderJag = new Jaguar(5);
minibot = new DoubleSolenoid(3, 4);cmp = new Compressor(DOC7_RELAY_PORT, DOC7_COMPRESSOR_PORT);
track = new Tracking(DOC7_LEFT_LINE_PORT, DOC7_CENTER_LINE_PORT, DOC7_RIGHT_LINE_PORT);
red_white = new Relay(DOC7_RED_WHITE_SPIKE);
red_white->SetDirection(Relay::kBothDirections);
red_white->Set(Relay::kOff);
blue = new Relay(DOC7_BLUE_SPIKE);
blue->SetDirection(Relay::kBothDirections);
blue->Set(Relay::kOff);
GetWatchdog().Kill();
};
