/*
* Handle the received message and implement the action.
*
* receivedMsgType the msgType, extracted from the
* received mesage.
* pReceivedMsgBody pointer to the body part of the
* received message.
* pSendMsg pointer to a message that we
* can fill in with the response.
*
* @return SERVER_SUCCESS_KEEP_RUNNING unless
* exitting in which case SERVER_EXIT_NORMALLY.
*/
static ServerReturnCode doAction (TaskHandlerMsgType receivedMsgType, UInt8 * pReceivedMsgBody, Msg *pSendMsg)
{
ServerReturnCode returnCode = SERVER_SUCCESS_KEEP_RUNNING;
Bool success = false;
ASSERT_PARAM (pReceivedMsgBody != PNULL, (unsigned long) pReceivedMsgBody);
ASSERT_PARAM (pSendMsg != PNULL, (unsigned long) pSendMsg);
/* We always respond with the same message type */
pSendMsg->msgType = (MsgType) receivedMsgType;
/* Fill in the length so far */
pSendMsg->msgLength += sizeof (pSendMsg->msgType);
/* Now handle each message specifically */
switch (receivedMsgType)
{
/*
* Messages to do with the server itself
*/
case TASK_HANDLER_SERVER_START:
{
success = initTaskList();
}
break;
case TASK_HANDLER_SERVER_STOP:
{
success = clearTaskList();
returnCode = SERVER_EXIT_NORMALLY;
}
break;
/*
* Messages to do with tasks
*/
case TASK_HANDLER_NEW_TASK:
{
success = handleNewTaskReq ((RoboOneTaskReq *) pReceivedMsgBody);
}
break;
case TASK_HANDLER_TICK:
{
success = tickTaskHandler();
}
break;
default:
{
ASSERT_ALWAYS_PARAM (receivedMsgType);
}
break;
}
/* Note: the following code assumes packing of 1 and that the start of a Cnf message contains the Bool 'success' */
pSendMsg->msgBody[0] = success;
pSendMsg->msgLength += sizeof (Bool);
return returnCode;
}
/*Moves the motors simultaneously the number of steps given in the array.
The motors will start and stop at the same time, no matter the selected
speed and number of steps (the motor that would take the longest is taken
as reference, and the speed of other motors adjusted accordingly).*/
int StepperControl::moveAll(long *stepsE){
movementCompleted = 0; //Resetting status.
int exitStatus = 0; //Return value of the external function that can be added to the loop.
uint16_t mask = 0; //Each bit contains info about what motor will be disabled (1 disabled, 0 enabled).
long steps[motorsCount]; //Local array containing the number of steps to be executed by each motor.
if(steps != NULL) memcpy(steps, stepsE, motorsCount*sizeof(long));
else return 2; //End function if the given array is not valid.
long minMovementDuration; //The lowest amount of time the slowest motor (or the one with the most steps to do) will need to complete it's movement.
long durations[motorsCount]; //Used for calculation of time-dependent values. Has different contents.
int slowestMotor = 0; //Index of the motor of which the movement will take the longest.
int dir[motorsCount]; //Contains movement direction of the motors.
totalSteps = 0; //Resetting the number of steps to be done.
int slMin; //Index of the motor with the lowest signalLength value among the active ones.
unsigned long *slp; //Points to a copy of signalLength.
slp = (unsigned long*)calloc(motorsCount, sizeof(unsigned long));
memcpy(slp, signalLength, motorsCount*sizeof(unsigned long)); //Sloppy (signalLength has type long*), but if signalLength contains negative values, there is a bigger problem.
//The direction pins of all motors are set here depending on whether or not the number of steps is negative.
//The total number of state changes to be done is also calculated here.
for(int i = 0; i < motorsCount; i++){
if(steps[i] < 0){
dir[i] = 1;
steps[i] = -steps[i];
}else{
dir[i] = 0;
}
totalSteps += steps[i];
setDir(i, dir[i]); //Setting the correct values to the direction pins.
durations[i] = movementTime[i] * steps[i];
if(!steps[i]){
mask |= (1 << i);
slp[i] = 0; //Causes arrayMin in the next step to ignore the motors that will be inactive.
}
}
totalSteps = totalSteps*2;
//Set starting conditions (without this part the signal polarity can be wrong).
slMin = arrayMin(slp, motorsCount, 0);
for(int i = 0; i < motorsCount; i++){
if(signalLength[i] > signalLength[slMin] && !(mask & (1 << i))){
stepStatus[i] = 1;
currentState[i] = 1;
}else{
stepStatus[i] = 0;
currentState[i] = 0;
}
}
free(slp);//We won't need slp any more.
//Here, the form and duration of the impulses is determined.
slowestMotor = arrayMax(durations, motorsCount);
minMovementDuration = durations[slowestMotor];
for(int i = 0; i < motorsCount; i++){
durations[i] = minMovementDuration / steps[i];
if(durations[i] > signalLength[i]) durations[i] -= signalLength[i];
else durations[i] = 0;
if(durations[i] > 0){
lowTime[i] = (long)((clockFrequency/(64*(1000000.0/durations[i]))));
highTime[i] = (long)((clockFrequency/(64*(1000000.0/signalLength[i]))));
}else{
lowTime[i] = 0;
highTime[i] = 0;
}
}
//Actual movement starts to happen here.
//Setting all motors to HIGH at start.
taskList[3][0] = 0xFFFF;
fillTaskList(mask);//Preparing the buffer.
enableInterrupt();//Starting movement.
while(!movementCompleted){
//if(idleCounter) Serial.println(idleCounter); //debug
exitStatus = repeatInLoop(); //External function to be executed during the movement.
if(exitStatus == 1) break; //If the external function returns 1, the movemen stops.
fillTaskList(mask);
}
disableInterrupt();//Stopping interrupts, so that they won't interfere with the program.
//Movement ended, resetting values.
stepsCounter = 0;
stepsCounterCalc = 0;
movementCompleted = 0;
executionIndex = 0;
calculationIndex = 1;
clearTaskList();
for(int i = 0; i < motorsCount; i++){
//individualStepsCounter[i] = 0; //Use resetIndividualStepsCounter() instead.
currentState[i] = 0;
stepStatus[i] = 0;
nextStep[i] = 0;
}
return exitStatus;
}
