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); } }