void UserInit(void){
//setPrintStream(&USBPutArray);
setPrintLevelInfoPrint();
println_I("\n\nStarting PIC initialization");
//DelayMs(1000);
hardwareInit();
println_I("Hardware Init done");
ReleaseAVRReset();
CheckRev();
LoadEEstore();
LoadDefaultValues();
CartesianControllerInit();
InitPID();
UpdateAVRLED();
lockServos();
setPrintLevelInfoPrint();
BOOL brown = getEEBrownOutDetect();
setCoProcBrownOutMode(brown);
setBrownOutDetect(brown);
println_I("###Starting PIC In Debug Mode###\n");// All printfDEBUG functions do not need to be removed from code if debug is disabled
DelayMs(1000);
//setPrintLevelErrorPrint();
println_E("Error level printing");
println_W("Warning level printing");
println_I("Info level printing");
}
void UserInit(void) {
//setPrintStream(&USBPutArray);
clearPrint();
setPrintLevelInfoPrint();
println_I("Start PIC");
//DelayMs(1000);
hardwareInit();
//println_I("Hardware Init done");
InitializeDyIODataTableManager();
CheckRev();
LoadEEstore();
UpdateAVRLED();
lockServos();
setPrintLevelInfoPrint();
boolean brown = getEEBrownOutDetect() ? true:false;
setBrownOutDetect(brown);
//Data table needs to be synced before the PID can init properly
SyncDataTable();
InitPID();
//println_I("###Starting PIC In Debug Mode###\n"); // All printfDEBUG functions do not need to be removed from code if debug is disabled
//DelayMs(1000);
setPrintLevelWarningPrint();
//println_E("Error level printing");
//println_W("Warning level printing");
//println_I("Info level printing");
}
void Collector::Idle(){
switch (CS) {
case ST::READY:
break;
case ST::SERVOS_LOCK:
lockServos();
stopFloor();
if (AllPurposeTimer->HasPeriodPassed(servoLockTime)) {
CS = ST::FLOOR_OPEN;
}
break;
case ST::FLOOR_OPEN:
openFloor();
if (isFloorOpen()) {
CS = ST::FLOOR_WAIT;
AllPurposeTimer->Reset();
}
break;
case ST::FLOOR_WAIT:
stopFloor();
if (AllPurposeTimer->HasPeriodPassed(floorWaitTime)) {
CS = ST::FLOOR_CLOSE;
}
break;
case ST::FLOOR_CLOSE:
closeFloor();
if (isFloorClosed()) {
CS = ST::SERVOS_UNLOCK;
AllPurposeTimer->Reset();
}
break;
case ST::SERVOS_UNLOCK:
unlockServos();
stopFloor();
if (AllPurposeTimer->HasPeriodPassed(servoUnlockTime)) {
CS = ST::READY;
}
break;
}
if (lockingServos) {
ServoLockRight->Set(1);
ServoLockLeft->Set(0);
}
else {
ServoLockRight->Set(0);
ServoLockLeft->Set(1);
}
if (closingFloor) {
if (isFloorClosed()) {
FloorDrive->Set(Relay::kOff);
closingFloor = false;
}
else FloorDrive->Set(Relay::kReverse);
}
else if (openingFloor) {
if (BottomFloorOpenSwitch->Get()) {
openingFloor = false;
FloorDrive->Set(Relay::kOff);
}
else FloorDrive->Set(Relay::kForward);
}
else {
FloorDrive->Set(Relay::kOff);
}
}
