/*! @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(); }