static void appTaskCanReceive(void *pdata) {
uint8_t error;
canMessage_t msg;
/* Install the CAN interrupt handler and start the OS ticker
* (must be done in the highest priority task)
*/
canRxInterrupt(canHandler);
osStartTick();
/*
* Now execute the main task loop for this task
*/
while ( true ) {
OSSemPend(can1RxSem, 0, &error);
msg = can1RxBuf;
interfaceLedToggle(D1_LED);
OSMutexPend(displayMutex, 0, &error);
lcdSetTextPos(2,1);
lcdWrite("ID : %08x", msg.id);
lcdSetTextPos(2,2);
lcdWrite("LEN : %08x", msg.len);
lcdSetTextPos(2,3);
lcdWrite("DATA_A : %08x", msg.dataA);
lcdSetTextPos(2,4);
lcdWrite("DATA_B : %08x", msg.dataB);
error = OSMutexPost(displayMutex);
}
}
static void appTaskLink(void *pdata) {
/* Start the OS ticker
* Must be done in the highest priority task
*/
osStartTick();
/* Task main loop */
while (true) {
ledToggle(USB_LINK_LED);
OSTimeDlyHMSM(0,0,0,500);
}
}
/*
* appTaskEmergencyStop - Emergency stop task
* This task handles the emergency stop and stop tasks in
* the robot 2 subsystem.
*/
static void appTaskEmergencyStop(void *pdata)
{
/* Start the OS ticker
* (must be done in the highest priority task)
*/
osStartTick();
canRxInterrupt(canHandler);
while(true)
{
if(emergencyStop)
{
ledToggle(USB_LINK_LED);
} else if (stopped) {
ledToggle(USB_CONNECT_LED);
} else {
OSTimeDlyHMSM(0,0,0,500);
}
}
}
/* Emergency Stop Task
* This task handles the pause, stop and emergency stop functions
* in the system.
*/
static void appTaskEmergencyStop(void *pdata)
{
// Start the OS ticker
osStartTick();
canRxInterrupt(canHandler);
while(true)
{
if(emergencyStop)
{
ledToggle(USB_LINK_LED);
conveyorSetState(CONVEYOR_OFF);
} else if(paused){
pausedState = conveyorGetState();
conveyorSetState(CONVEYOR_OFF);
} else if (stopped){
ledToggle(USB_CONNECT_LED);
}
else{
OSTimeDlyHMSM(0,0,0,500);
}
}
}
static void appTaskButtons(void *pdata) {
uint32_t btnState;
static uint8_t joint = ROBOT_HAND;
static uint32_t leds[5] = {D1_LED, D1_LED, D2_LED, D3_LED, D4_LED};
static bool jsRightPressed = false;
static bool jsLeftPressed = false;
/* Start the OS ticker (highest priority task) */
osStartTick();
/* Initialise the display */
lcdSetTextPos(2, 7);
lcdWrite("HAND :%08u", robotJointGetState(ROBOT_HAND));
lcdSetTextPos(2, 8);
lcdWrite("WRIST:%08u", robotJointGetState(ROBOT_WRIST));
lcdSetTextPos(2, 9);
lcdWrite("ELBOW:%08u", robotJointGetState(ROBOT_ELBOW));
lcdSetTextPos(2, 10);
lcdWrite("WAIST:%08u", robotJointGetState(ROBOT_WAIST));
/* the main task loop for this task */
while (true) {
btnState = buttonsRead();
if (isButtonPressedInState(btnState, JS_RIGHT)) {
jsRightPressed = true;
}
if (jsRightPressed && (!isButtonPressedInState(btnState, JS_RIGHT))) {
jsRightPressed = false;
interfaceLedSetState(leds[joint], LED_OFF);
joint += 1;
if (joint > N_JOINTS) {
joint = 1;
}
}
if (isButtonPressedInState(btnState, JS_LEFT)) {
jsLeftPressed = true;
}
if (jsLeftPressed && (!isButtonPressedInState(btnState, JS_LEFT))) {
jsLeftPressed = false;
interfaceLedSetState(leds[joint], LED_OFF);
joint -= 1;
if (joint == 0) {
joint = N_JOINTS;
}
}
interfaceLedSetState(leds[joint], LED_ON);
if (isButtonPressedInState(btnState, JS_UP)) {
robotJointSetState((robotJoint_t)joint, ROBOT_JOINT_POS_INC);
lcdSetTextPos(8, 6+joint);
lcdWrite("%08u", robotJointGetState((robotJoint_t)joint));
} else if (isButtonPressedInState(btnState, JS_DOWN)) {
robotJointSetState((robotJoint_t)joint, ROBOT_JOINT_POS_DEC);
lcdSetTextPos(8, 6+joint);
lcdWrite("%08u", robotJointGetState((robotJoint_t)joint));
}
if (isButtonPressedInState(btnState, BUT_1)) {
robotJointSetState(ROBOT_WAIST, 72250);
}
OSTimeDly(20);
}
}
