// Perform homing cycle to locate and set machine zero. Only '$H' executes this command.
// NOTE: There should be no motions in the buffer and Grbl must be in an idle state before
// executing the homing cycle. This prevents incorrect buffered plans after homing.
void MotionGoHome(void)
{
plan_init();
uint16_t limitStatus;
uint16_t stepCount;
sys.state = STATE_HOMING; // Set system state variable
// LIMIT_PCMSK &= ~LIMIT_MASK; // Disable hard limits pin change register for cycle duration
xLimitDetected = FALSE;
yLimitDetected = FALSE;
BSP_SetStepSize(X_AXIS, QUARTER);
BSP_SetStepSize(Y_AXIS, QUARTER);
BSP_SetStepSize(Z_AXIS, QUARTER);
settings.steps_per_mm[X_AXIS] = X_STEPS_MM(QUARTER_STEP);
settings.steps_per_mm[Y_AXIS] = Y_STEPS_MM(QUARTER_STEP);
settings.steps_per_mm[Z_AXIS] = Z_STEPS_MM(QUARTER_STEP);
// Find out if X or Y axis are at limit
limitStatus = mPORTCReadBits(xLimitInput.pin||yLimitInput.pin);
if(limitStatus)
{
if(limitStatus & xLimitInput.pin) // If Xat Limit
{
stepCount = 0;
BSP_Timer3Start(100);
OpenOC2(XS_PWM_ENA, (ReadPeriod3()>>1), ReadPeriod3()>>1);
BSP_AxisEnable(X_AXIS, NEGATIVE);
while((mPORTCReadBits(xLimitInput.pin)))
{
stepCount++;
steps_X = 0x1;
if(stepCount >= 20)
mPORTGToggleBits(xAxis.directionPin.pin);
}
BSP_AxisDisable(X_AXIS);
if(mPORTGReadBits(xAxis.directionPin.pin) == POSITIVE)
gcode.position[X_AXIS] = 215.000;
else
gcode.position[X_AXIS] =0;
}
if(limitStatus & yLimitInput.pin) // If Xat Limit
{
stepCount = 0;
BSP_Timer2Start(100);
OpenOC1(XS_PWM_ENA, (ReadPeriod2()>>1), ReadPeriod2()>>1);
BSP_AxisEnable(Y_AXIS, NEGATIVE);
while((mPORTCReadBits(yLimitInput.pin)))
{
stepCount++;
steps_Y = 0x1;
if(stepCount >= 20)
mPORTEToggleBits(yAxis.directionPin.pin);
}
if(mPORTEReadBits(xAxis.directionPin.pin) == POSITIVE)
gcode.position[Y_AXIS] = 215.000;
else
gcode.position[Y_AXIS] =0;
}
/*
* Helper function for getting timer periods.
* The timer period is the maximum value for a output compare register.
* Setting the OCRn register to the timer period results in the fastest
* motor speed.
*/
static uint32_t get_timer_period(struct hbridge_info* hbridge)
{
uint32_t retval = 0;
switch(hbridge->tmrn)
{
case 1:
retval = ReadPeriod1();
break;
case 2:
retval = ReadPeriod2();
break;
case 3:
retval = ReadPeriod3();
break;
case 4:
retval = ReadPeriod4();
break;
case 5:
retval = ReadPeriod5();
break;
}
}
/********************************************************************
* Tache: void TaskControl(void *pvParameters)
*
* Overview: Tâche responsable des différents lois de commande.
* Asservissement de position pour la direction, asservissement de
* courant (couple) pour la propulsion et supervision du niveau de
* batterie. Une fois les traitement fais, elle est responsable du
* post des grandeurs intermédaire pour le debug à la tâche d'affichage
*
* Auteur Date Commentaire
*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
* Théou Jean-Baptiste 7 avril 2011 Première version
* Descoubes hugo 16 mai 2011 vs 1.1
*******************************************************************/
void TaskControl(void *pvParameters){
/* asservissement de position */
short sDirMeasure; // Mesure de position (servomoteur). Unsigned integer sur 10 bits
short sDirConsigne; // Consigne de position (servomoteur). Unsigned integer sur 10 bits
short sDirCommand; // Commande de position (servomoteur). entier compris entre 0 et 100, rapport cyclique pour PWM
/* asservissement de courant */
float fPropMeasure; // Mesure image du courant absorbé par la MCC (propulsion).
short sPropConsigne; // Consigne de courant(propulsion). Unsigned integer sur 10 bits
short sPropCommand; // Commande de courant (propulsion). entier compris entre 0 et 100, rapport cyclique pour PWM
short sPowerMeasure; // Mesure du niveau de batterie. Unsigned integer sur 10 bits
char stateControl = PROPULSION_CONTROL;// machnie d'état pour la commande
struct_DebugPrint printData; // Données à afficher (debug)
struct_mesures measuresReceive; // Mesures reçues depuis l'ISR du timer 3
for( ;; ){
/* Récupération des grandeurs de mesures pour les lois de commande */
xQueueReceive( xQueueAcquiData, &measuresReceive, portMAX_DELAY); // fonction bloquante
/* Mise à jour mesures */
sDirMeasure = measuresReceive.sDirMeasure;
fPropMeasure = (float) measuresReceive.sCurrentMeasure / 35.0; // 35 = facteur d'échelle (capteur + conditionneur + ADC)
sPowerMeasure = measuresReceive.sBattMeasure;
/* Mise à jour mesures - Affichage par le réseau */
resultat = sDirMeasure; // Mise à jour var. globale réseau
resultat_courant = measuresReceive.sCurrentMeasure; // Mise à jour var. globale réseau
/* Mise à jour consignes */
/* Protection Vitesse*/
xSemaphoreTake(xSemaphoreVitesse,portMAX_DELAY);
{
sPropConsigne = Vitesse; // récupération var. globale réseau
}
xSemaphoreGive(xSemaphoreVitesse);
/* Protection ConsDir */
xSemaphoreTake(xSemaphoreConsDir,portMAX_DELAY);
{
sDirConsigne = ConsDir; // récupération var. globale réseau
}
xSemaphoreGive(xSemaphoreConsDir);
/* Machine d'état - tâche contrôle/commande/supervision */
switch(stateControl){
case PROPULSION_CONTROL :
/* Protection Consigne courant */
if(sPropConsigne > 100){
sPropConsigne = 100;
}
else if(sPropConsigne < 0){
sPropConsigne = 0;
}
/* Asservissement de courant (couple). Régulation PI (modèle non-linéaire) */
/* Calcul fais avec des flottant - temps de traitement long */
sPropCommand = propulsionCommand (sPropConsigne , fPropMeasure);
case PROPULSION_PWM :
if((fPropMeasure < 0.0) || (fPropMeasure > MAX_CURRENT) ){
sPropCommand = 0;
}
/* Mise à jour rapport cyclique pour module PWM propulsion */
/* Protection au niveau des erreurs de calculs */
if(sPropCommand < 10)
{
sPropCommand = 0;
}
SetDCOC4PWM(ReadPeriod3() * sPropCommand / 100 );
case DIRECTION_CONTROL :
/* Protection Consigne direction */
if(sDirConsigne > HAUT_BUTE){
sDirConsigne = HAUT_BUTE;
}
else if(sDirConsigne < BAS_BUTE){
sDirConsigne = BAS_BUTE;
}
/* Limitation sur la var globale ConsDir fait localement sur le MCU ... pour le moment ! */
/* Protection ConsDir */
xSemaphoreTake(xSemaphoreConsDir,portMAX_DELAY);
{
ConsDir = sDirConsigne;
}
xSemaphoreGive(xSemaphoreConsDir);
/* Asservissement de position. Régulation proporionnelle (modèle grandement non-linéaire) */
sDirCommand = directionCommand(sDirConsigne, sDirMeasure);
case DIRECTION_PWM :
/* Vérification butées direction */
if( sDirMeasure < HAUT_BUTE && sDirMeasure > BAS_BUTE){
if(init_ok == 0){
/* Attendre initialisation utilisateur via réseau */
sDirCommand = 0;
}
else{
/* valeur absolue et sens de rotation - PWM comprise entre 0 et 100 */
if ( sDirCommand < 0 ){
/* Sens de rotation pour le driver de bras de pont */
PORTSetBits(BROCHE_DIR_DIRECTION);
sDirCommand = -sDirCommand;
}
else{
/* Sens de rotation pour le driver de bras de pont */
PORTClearBits(BROCHE_DIR_DIRECTION);
}
/* Protection et limitation Commande */
if(sDirCommand < 20){
sDirCommand = 0; // Pas de commande si dans dead zone
}
else if(sDirCommand < 30){
sDirCommand = 40; // compensation dead zone
}
else if (sDirCommand > 100){
sDirCommand = 100; // limitation PWM
}
}
}
else{
sDirCommand = 0;
}
/* Mise à jour rapport cyclique pour module PWM direction */
SetDCOC2PWM(ReadPeriod3() * sDirCommand / 100 );
case POWER_SUPERVIOR :
break;
default:
break;
}
/* Post les valeurs à afficher pour le debug vers la tâche d'affichage */
printData.commandeDir = sDirCommand;
printData.mesureDir = sDirMeasure;
printData.consigneDir = sDirConsigne;
printData.consigneMove = (float) sPropConsigne;
printData.mesureMove = fPropMeasure;
printData.commandeMove = sPropCommand;
printData.mesurePower = sPowerMeasure;
xQueueSend(xQueueDebugPrint, &printData, 0);
}
}
//Main Timer for total movement time and block handling
void __ISR(_TIMER_1_VECTOR, ipl3) _InterruptHandler_TMR1(void)
{
// clear the interrupt flag
mT1ClearIntFlag();
if(current_block == Null)
{
current_block = plan_get_current_block();
if(current_block != Null)
{
if(current_block->activeAxisCount == 3)
{
if(current_block->minStepAxis < N_AXIS) // If they are not all equal // TODO: If any of the step counts are equal we dont need Timer4
{
switch(current_block->minStepAxis) // Need to configure OC Module to be Single Pulse Output and other two OC modules to be continuous pulse
{
case X_AXIS:
BSP_Timer4Start((uint16_t)current_block->steppingFreq[X_AXIS]);// Use Timer4 Interrupt to trigger single output pulse
OpenOC2((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER3_SRC|OC_SINGLE_PULSE), (ReadPeriod3()>>1), ReadPeriod3()); // X_AXIS = Single Pulse
BSP_Timer2Start((uint16_t)current_block->steppingFreq[Y_AXIS]);
OpenOC1((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER2_SRC|OC_CONTINUE_PULSE), (ReadPeriod2()>>1), ReadPeriod2()); // Y_AXIS = Continuous Pulse
BSP_Timer3Start((uint16_t)current_block->steppingFreq[Z_AXIS]);
OpenOC3((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER3_SRC|OC_CONTINUE_PULSE), (ReadPeriod3()>>1), ReadPeriod3()); // Z_AXIS = Continuous Pulse
break;
case Y_AXIS:
BSP_Timer4Start((uint16_t)current_block->steppingFreq[Y_AXIS]);// Use Timer4 Interrupt to trigger single output pulse
OpenOC1((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER3_SRC|OC_SINGLE_PULSE), (ReadPeriod3()>>1), ReadPeriod3()); // Y_AXIS = Single Pulse
BSP_Timer2Start((uint16_t)current_block->steppingFreq[X_AXIS]);
OpenOC2((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER2_SRC|OC_CONTINUE_PULSE), (ReadPeriod2()>>1), ReadPeriod2()); // X_AXIS = Continuous Pulse
BSP_Timer3Start((uint16_t)current_block->steppingFreq[Z_AXIS]);
OpenOC3((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER3_SRC|OC_CONTINUE_PULSE), (ReadPeriod3()>>1), ReadPeriod3()); // Z_AXIS = Continuous Pulse
break;
case Z_AXIS:
BSP_Timer4Start((uint16_t)current_block->steppingFreq[Z_AXIS]);// Use Timer4 Interrupt to trigger single output pulse
OpenOC3((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER3_SRC|OC_SINGLE_PULSE), (ReadPeriod3()>>1), ReadPeriod3()); // Z_AXIS = Single Pulse
BSP_Timer3Start((uint16_t)current_block->steppingFreq[X_AXIS]);
OpenOC2((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER3_SRC|OC_CONTINUE_PULSE), (ReadPeriod3()>>1), ReadPeriod3()); // X_AXIS = Continuous Pulse
BSP_Timer2Start((uint16_t)current_block->steppingFreq[Y_AXIS]);
OpenOC1((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER2_SRC|OC_CONTINUE_PULSE), (ReadPeriod2()>>1), ReadPeriod2()); // Y_AXIS = Continuous Pulse
break;
default:
// error
break;
}
}
else
{
BSP_Timer2Start((uint16_t)current_block->steppingFreq[X_AXIS]); // All Steps Counts are equal so just use on timer
OpenOC1((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER2_SRC|OC_CONTINUE_PULSE), (ReadPeriod2()>>1), ReadPeriod2()); // Y_AXIS = Continuous Pulse
OpenOC2((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER2_SRC|OC_CONTINUE_PULSE), (ReadPeriod2()>>1), ReadPeriod2()); // X_AXIS = Continuous Pulse
OpenOC3((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER2_SRC|OC_CONTINUE_PULSE), (ReadPeriod2()>>1), ReadPeriod2()); // Z_AXIS = Continuous Pulse
}
BSP_AxisEnable(Y_AXIS, current_block->direction_bits[Y_AXIS]);
BSP_AxisEnable(X_AXIS, current_block->direction_bits[X_AXIS]);
BSP_AxisEnable(Z_AXIS, current_block->direction_bits[Z_AXIS]);
}
else if (current_block->activeAxisCount == 2) // 2 Axis Enabled
