161 lines
5.1 KiB
C
161 lines
5.1 KiB
C
//=============================================================================
|
|
// Demo program "Motor run".
|
|
//
|
|
// Can be run under control of the ROBO TX Controller
|
|
// firmware in download (local) mode.
|
|
// Controls motor connected to outputs M1 with different
|
|
// speeds and in different rotation directions.
|
|
//
|
|
// Disclaimer - Exclusion of Liability
|
|
//
|
|
// This software is distributed in the hope that it will be useful, but WITHOUT
|
|
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
|
// FITNESS FOR A PARTICULAR PURPOSE. It can be used and modified by anyone
|
|
// free of any license obligations or authoring rights.
|
|
//=============================================================================
|
|
|
|
#include "ROBO_TX_PRG.h"
|
|
|
|
#define TA_IDX TA_LOCAL
|
|
#define MOTOR_NUMBER 1
|
|
#define MOTOR_IDX (MOTOR_NUMBER - 1)
|
|
|
|
static unsigned long timer;
|
|
static enum {DIRECTION_1, PAUSE, DIRECTION_2, OFF} stage;
|
|
static int duty;
|
|
|
|
|
|
/*-----------------------------------------------------------------------------
|
|
* Function Name : PrgInit
|
|
*
|
|
* This it the program initialization.
|
|
* It is called once.
|
|
*-----------------------------------------------------------------------------*/
|
|
void PrgInit
|
|
(
|
|
TA * p_ta_array, // pointer to the array of transfer areas
|
|
int ta_count // number of transfer areas in array (equal to TA_COUNT)
|
|
)
|
|
{
|
|
TA * p_ta = &p_ta_array[TA_IDX];
|
|
|
|
// Configure motor output to be used as a motor output
|
|
p_ta->config.motor[MOTOR_IDX] = TRUE;
|
|
|
|
// Inform firmware that configuration was changed
|
|
p_ta->state.config_id += 1;
|
|
|
|
// Switch motor off
|
|
p_ta->output.duty[2 * MOTOR_IDX + 0] = 0;
|
|
p_ta->output.duty[2 * MOTOR_IDX + 1] = 0;
|
|
|
|
timer = 0;
|
|
stage = DIRECTION_1; // motor should start to rotate in the first direction
|
|
duty = 0;
|
|
}
|
|
|
|
|
|
/*-----------------------------------------------------------------------------
|
|
* Function Name : PrgTic
|
|
*
|
|
* This is the main function of this program.
|
|
* It is called every tic (1 ms) realtime.
|
|
*-----------------------------------------------------------------------------*/
|
|
int PrgTic
|
|
(
|
|
TA * p_ta_array, // pointer to the array of transfer areas
|
|
int ta_count // number of transfer areas in array (equal to TA_COUNT)
|
|
)
|
|
{
|
|
int rc = 0x7FFF; // return code: 0x7FFF - program should be further called by the firmware;
|
|
// 0 - program should be normally stopped by the firmware;
|
|
// any other value is considered by the firmware as an error code
|
|
// and the program is stopped.
|
|
TA * p_ta = &p_ta_array[TA_IDX];
|
|
|
|
while (1)
|
|
{
|
|
switch (stage)
|
|
{
|
|
case DIRECTION_1:
|
|
if (timer == 0)
|
|
{
|
|
// Switch motor on in the first direction with the speed from the duty value
|
|
duty += DUTY_MAX / 4;
|
|
p_ta->output.duty[2 * MOTOR_IDX + 0] = duty;
|
|
p_ta->output.duty[2 * MOTOR_IDX + 1] = 0;
|
|
}
|
|
if (++timer >= 2000) // motor should rotate with such a speed for 2 seconds
|
|
{
|
|
timer = 0;
|
|
if (duty >= DUTY_MAX)
|
|
{
|
|
stage = PAUSE;
|
|
}
|
|
else
|
|
{
|
|
continue;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return rc;
|
|
}
|
|
|
|
case PAUSE:
|
|
if (timer == 0)
|
|
{
|
|
// Switch motor off
|
|
p_ta->output.duty[2 * MOTOR_IDX + 0] = 0;
|
|
p_ta->output.duty[2 * MOTOR_IDX + 1] = 0;
|
|
}
|
|
if (++timer >= 2000) // motor should be stopped for 2 seconds
|
|
{
|
|
stage = DIRECTION_2;
|
|
timer = 0;
|
|
}
|
|
else
|
|
{
|
|
return rc;
|
|
}
|
|
|
|
case DIRECTION_2:
|
|
if (timer == 0)
|
|
{
|
|
// Switch motor on in the second direction with the speed from the duty value
|
|
p_ta->output.duty[2 * MOTOR_IDX + 0] = 0;
|
|
p_ta->output.duty[2 * MOTOR_IDX + 1] = duty;
|
|
}
|
|
if (++timer >= 2000) // motor should rotate with such a speed for 2 seconds
|
|
{
|
|
duty -= DUTY_MAX / 4;
|
|
if (duty <= 0)
|
|
{
|
|
stage = OFF;
|
|
}
|
|
else
|
|
{
|
|
timer = 0;
|
|
continue;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return rc;
|
|
}
|
|
|
|
case OFF:
|
|
// Switch motor off
|
|
p_ta->output.duty[2 * MOTOR_IDX + 0] = 0;
|
|
p_ta->output.duty[2 * MOTOR_IDX + 1] = 0;
|
|
|
|
rc = 0; // stop program
|
|
return rc;
|
|
|
|
default:
|
|
return rc;
|
|
}
|
|
}
|
|
return rc;
|
|
}
|