bool openCubesCatcher()
{
setServoAngle((uint8_t)ID_RIGHT, (uint16_t)OPEN_ANG_RIGHT);
setServoAngle((uint8_t)ID_LEFT, (uint16_t)OPEN_ANG_LEFT);
softDelay(500);
// duplicate
setServoAngle((uint8_t)ID_RIGHT, (uint16_t)OPEN_ANG_RIGHT);
setServoAngle((uint8_t)ID_LEFT, (uint16_t)OPEN_ANG_LEFT);
return 0;
}
int main() {
GPIO_InitTypeDef gpio;
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOE, ENABLE);
gpio.GPIO_Mode = GPIO_Mode_OUT;
gpio.GPIO_OType = GPIO_OType_PP;
gpio.GPIO_Speed = GPIO_Speed_Level_3;
gpio.GPIO_PuPd = GPIO_PuPd_UP;
gpio.GPIO_Pin = GPIO_Pin_10;
GPIO_Init(GPIOE, &gpio);
while(1) {
GPIO_SetBits(GPIOE, GPIO_Pin_10);
softDelay(10000000);
GPIO_ResetBits(GPIOE, GPIO_Pin_10);
softDelay(10000000);
}
}
void ReserveCell(unsigned char x,unsigned char y)
{//Cell required, so tell other mice to reserve cells
SetCellReserved(x,y);
while(!CTS);
dint();
AssembleCellReservePacket(x,y);
rfStandbyMode();
rfSendPacket2Module(&TxPacket[0],32);
rfTxMode();
eint();
softDelay(65535);
}
bool closeCubesCatcher(uint8_t *numberOfCubesCatched)
{
setServoAngle((uint8_t)ID_RIGHT, (uint16_t)CLOSED_ANG_RIGHT);
setServoAngle((uint8_t)ID_LEFT, (uint16_t)CLOSED_ANG_LEFT);
softDelay(500);
// duplicate
setServoAngle((uint8_t)ID_RIGHT, (uint16_t)CLOSED_ANG_RIGHT);
setServoAngle((uint8_t)ID_LEFT, (uint16_t)CLOSED_ANG_LEFT);
softDelay(20000000);
while((prev_right_servo_angle != right_servo_angle) || (prev_left_servo_angle != left_servo_angle))
{
prev_right_servo_angle = right_servo_angle;
prev_left_servo_angle = left_servo_angle;
getServoAngle((uint8_t)ID_RIGHT, &right_servo_angle);
softDelay(1000);
getServoAngle((uint8_t)ID_LEFT, &left_servo_angle);
}
float difference = right_servo_angle - left_servo_angle;
if ((difference > 1) && (difference <= 15))
{
*numberOfCubesCatched = 1;
}
else if ((difference > 15))
{
*numberOfCubesCatched = 2;
}
else
*numberOfCubesCatched = 0; // no cubes were caught or number of cubes is unknown
right_servo_angle = 0;
left_servo_angle = 0;
prev_right_servo_angle = 1;
prev_left_servo_angle = 1;
return 0;
}
