/*!
@brief Thread that controls movement of arm and magnet state.
@param argument Unused
@retval None.
*/
void armThread (void const *argument) {
uint8_t data[RECEIVE_DATA_SIZE];
parkRobot(&robot);
osDelay(1000);
// uint32_t prevY1=0, prevAngle1=0, nextY1=rand()%7, nextAngle1=rand()%14;
// uint32_t prevY2=0, prevAngle2=0, nextY2=rand()%7, nextAngle2=rand()%14;
while(1) {
wireless_RX(&receiver);
osMutexWait(receiver.mutexID, osWaitForever);
for (uint32_t i=0; i<sizeof(data)/sizeof(data[0]); i++) {
data[i] = receiver.data[i];
}
osMutexRelease(receiver.mutexID);
moveRobot(&robot, data[0], data[1], data[2]);
if (data[3]) {
turnMagnetOn(&magnet);
} else {
turnMagnetOff(&magnet);
}
// moveRobot(&robot, prevY1, 0, prevAngle1);
// turnMagnetOn(&magnet);
// osDelay(500);
// moveRobot(&robot, nextY1, 0, nextAngle1);
// turnMagnetOff(&magnet);
// osDelay(500);
//
// moveRobot(&robot, prevY2, 0, prevAngle2);
// turnMagnetOn(&magnet);
// osDelay(500);
// moveRobot(&robot, nextY2, 0, nextAngle2);
// turnMagnetOff(&magnet);
// osDelay(500);
//
// prevY1=nextY1;
// prevAngle1=nextAngle1;
// prevY2=nextY2;
// prevAngle2=nextAngle2;
//
// nextY1=rand()%7;
// nextAngle1=rand()%14;
// nextY2=rand()%7;
// nextAngle2=rand()%14;
osDelay(100);
}
}
/*
Used to commence scoring with the block and parking based on the values
Parameters:
distance - distance to travel
speed - speed to travel
direction - direction to travel
*/
void scoreRobot(float distance, float speed, string direction){
scoreBlock();
moveRobot(distance, speed, direction);
parkRobot();
stopRobot();
}
