96 lines
3.4 KiB
C
96 lines
3.4 KiB
C
|
//=============================================================================
|
||
|
|
// Demo program "Extended motor control mode on Extension 1".
|
||
|
|
//
|
||
|
|
// Can be run under control of the ROBO TX Controller
|
||
|
|
// firmware in download (local) mode.
|
||
|
|
// Continuously starts a motor with changing rotation directions.
|
||
|
|
// Motor is connected to outputs M1. Pulses from the motor are
|
||
|
|
// calculated by the counter C1. The motor is stopped and rotation
|
||
|
|
// direction is changed after the counter reaches the value of 200.
|
||
|
|
//
|
||
|
|
// 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_EXT_1
|
||
|
|
#define MOTOR_NUMBER 1
|
||
|
|
#define MOTOR_IDX (MOTOR_NUMBER - 1)
|
||
|
|
|
||
|
|
static BOOL32 rotation_direction;
|
||
|
|
static BOOL32 prev_rotation_direction;
|
||
|
|
|
||
|
|
|
||
|
|
/*-----------------------------------------------------------------------------
|
||
|
|
* 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;
|
||
|
|
|
||
|
|
rotation_direction = FALSE;
|
||
|
|
prev_rotation_direction = !rotation_direction;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/*-----------------------------------------------------------------------------
|
||
|
|
* 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];
|
||
|
|
|
||
|
|
if (rotation_direction != prev_rotation_direction)
|
||
|
|
{
|
||
|
|
// Set motor duty depending on rotation_direction
|
||
|
|
p_ta->output.duty[2 * MOTOR_IDX + 0] = (rotation_direction) ? DUTY_MAX : 0;
|
||
|
|
p_ta->output.duty[2 * MOTOR_IDX + 1] = (rotation_direction) ? 0 : DUTY_MAX;
|
||
|
|
|
||
|
|
// Motor should run until counter reaches 200
|
||
|
|
p_ta->output.distance[MOTOR_IDX] = 200;
|
||
|
|
|
||
|
|
p_ta->input.motor_pos_reached[MOTOR_IDX] = FALSE;
|
||
|
|
|
||
|
|
// Start extended motor control command
|
||
|
|
p_ta->output.motor_ex_cmd_id[MOTOR_IDX]++;
|
||
|
|
|
||
|
|
prev_rotation_direction = rotation_direction;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (p_ta->input.motor_pos_reached[MOTOR_IDX])
|
||
|
|
{
|
||
|
|
rotation_direction = !rotation_direction;
|
||
|
|
}
|
||
|
|
|
||
|
|
return rc;
|
||
|
|
}
|