void ConfigPPM(BowlerPacket * Packet){
int i;
for(i=0;i<NUM_PPM_CHAN;i++){
ppmLink[i]=Packet->use.data[i+1];
if(ppmLink[i]>= GetNumberOfIOChannels()){
ppmLink[i]=INVALID_PPM_LINK;
}else{
unlockServos();
}
}
writeLinks=true;
READY(Packet,66,0);
}
void ConfigPPMFromArray(uint8_t * data){
int i;
for(i=0;i<NUM_PPM_CHAN;i++){
ppmLink[i]=data[i];
if(ppmLink[i]>= GetNumberOfIOChannels()){
ppmLink[i]=INVALID_PPM_LINK;
}else{
unlockServos();
}
}
writeLinks=true;
}
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);
}
}
