A port of the smart motor library for ChibiOS/RT. More...

#include <stdlib.h>
#include <math.h>
#include "smartmotor.h"
#include "robotc_glue.h"
#include "fastmath.c"

Functions
smartMotor *	SmartMotorGetPtr (tVexMotor index)
	Get pointer to smartMotor structure - not used locally. More...

smartController *	SmartMotorControllerGetPtr (short index)
	Get pointer to smartController structure - not used locally. More...

float	SmartMotorGetSpeed (tVexMotor index)
	Get Motor speed. More...

float	_SmartMotorGetCurrent (tVexMotor index, int s,...)
	Get Motor current. More...

float	SmartMotorGetTemperature (tVexMotor index)
	Get Motor temperature. More...

int	SmartMotorGetLimitCmd (tVexMotor index)
	Get Motor command limit. More...

void	_SmartMotorSetLimitCurent (tVexMotor index, float current,...)
	Set Motor current limit. More...

void	SmartMotorSetFreeRpm (tVexMotor index, short max_rpm)
	Set Motor maximum rpm. More...

void	SmartMotorSetSlewRate (tVexMotor index, int slew_rate)
	Set Motor slew rate. More...

float	SmartMotorGetControllerCurrent (short index)
	Get Controller current. More...

float	SmartMotorGetControllerTemperature (short index)
	Get Controller temperature. More...

void	SmartMotorSetControllerStatusLed (int index, tVexDigitalPin port)
	Set Controller status LED. More...

void	SmartMotorSetPowerExpanderStatusPort (tVexAnalogPin port)
	Set power expander status port. More...

void	SmartMotorPtcMonitorEnable ()
	Enable current limit by monitoring the PTC temperatures. More...

void	SmartMotorPtcMonitorDisable ()
	Disable current limit by monitoring the PTC temperatures. More...

void	SmartMotorCurrentMonitorEnable ()
	Enable current limit by using a preset threshold. More...

void	SmartMotorCurrentMonitorDisable ()
	Disable current limit by using a preset threshold. More...

void	SmartMotorRun ()
	Start the smart motor monitoring. More...

void	SmartMotorStop ()
	Stop smart motor tasks. More...

void	SmartMotorDebugStatus ()
	Dump controller and motor status to the debug stream. More...

void	_SetMotor (int index, int value, bool_t immediate,...)
	Set motor to value. More...

void	SmartMotorsInit ()
	Initialize the smartMotor library. More...

void	SmartMotorLinkMotors (tVexMotor master, tVexMotor slave)
	Link two motors. More...

void	SmartMotorsSetEncoderGearing (tVexMotor index, float ratio)
	Set the encoder gearing. More...

void	_SmartMotorsAddPowerExtender (int p0, int p1, int p2, int p3,...)
	Add a power extender to the system. More...

void	_SmartMotorSetRpmSensor (tVexMotor index, tVexAnalogPin port, float ticks_per_rev, bool_t reversed,...)
	Assign (non encoder) sensor for rpm measurement. More...

void	SmartMotorSpeed (smartMotor *m, int deltaTime)
	calculate speed in rpm for the motor More...

void	SmartMotorSimulateSpeed (smartMotor *m)
	calculate a simulated speed in rpm for a motor More...

float	SmartMotorCurrent (smartMotor *m, float v_battery)
	Estimate smart motor current. More...

float	SmartMotorControllerCurrent (smartController *s)
	Calculate the current for a controller bank. More...

int	SmartMotorSafeCommand (smartMotor *m, float v_battery)
	Calculate safe current command. More...

float	SmartMotorTemperature (smartMotor *m, int deltaTime)
	Calculate the PTC temperature for a motor. More...

float	SmartMotorControllerTemperature (smartController *s, int deltaTime)
	Calculate the PTC temperature for a controller bank. More...

void	SmartMotorMonitorPtc (smartMotor *m, float v_battery)
	Monitor Motor PTC temperature. More...

void	SmartMotorControllerMonitorPtc (smartController *s, float v_battery)
	Monitor controller bank PTC temperature. More...

void	SmartMotorMonitorCurrent (smartMotor *m, float v_battery)
	Monitor Motor Current. More...

void	SmartMotorControllerSetLed (smartController *s)
	Set the current monitor LED on or off. More...

msg_t	SmartMotorTask (void *arg)
	The smart motor task. More...

msg_t	SmartMotorSlewRateTask (void *arg)
	The motor slew rate task. More...

Detailed Description

A port of the smart motor library for ChibiOS/RT.

Function Documentation

smartMotor* SmartMotorGetPtr ( tVexMotor index)

Get pointer to smartMotor structure - not used locally.

Parameters

[in] index The motor index

smartController* SmartMotorControllerGetPtr ( short index)

Get pointer to smartController structure - not used locally.

Parameters

[in] index The motor index

float SmartMotorGetSpeed ( tVexMotor index)

Get Motor speed.

Parameters

[in] index The motor index

Returns: The motor speed as rpm

Examples:: clawbot.c.

float _SmartMotorGetCurrent	(	tVexMotor	index,
		int	s,
			...
	)

Get Motor current.

Parameters

[in]	index	The motor index
[in]	s	Set as 1 for signed current otherwise absolute is returned

Returns: The motor current in amps

float SmartMotorGetTemperature ( tVexMotor index)

Get Motor temperature.

Parameters

[in] index The motor index

Returns: The motor temperature in deg C

int SmartMotorGetLimitCmd ( tVexMotor index)

Get Motor command limit.

Parameters

[in] index The motor index

Returns: The motor maximum command value

void _SmartMotorSetLimitCurent	(	tVexMotor	index,
		float	current,
			...
	)

Set Motor current limit.

Parameters

[in]	index	The motor index
[in]	current	The maximum motor current

Note: Do not use this function directly, instead use the macro SmartMotorSetLimitCurent which allows a default value for current of 1.0A

void SmartMotorSetFreeRpm	(	tVexMotor	index,
		short	max_rpm
	)

Set Motor maximum rpm.

Parameters

[in]	index	The motor index
[in]	max_rpm	The maximum motor rpm

void SmartMotorSetSlewRate	(	tVexMotor	index,
		int	slew_rate
	)

Set Motor slew rate.

Parameters

[in]	index	The motor index
[in]	slew_rate	The maximum motor slew_rate

float SmartMotorGetControllerCurrent ( short index)

Get Controller current.

Parameters

[in] index The motor controller index (0, 1 or 2)

Returns: The motor controller current in amps

float SmartMotorGetControllerTemperature ( short index)

Get Controller temperature.

Parameters

[in] index The motor controller index (0, 1 or 2)

Returns: The motor controller temperature in deg C

void SmartMotorSetControllerStatusLed	(	int	index,
		tVexDigitalPin	port
	)

Set Controller status LED.

Parameters

[in]	index	The motor controller index (0, 1 or 2)
[in]	port	The digital port that the led is connected to

This function allows an led to be assigned to a controller bank that is illuminated when any motor on that bank trips the ptc or exceeds the preset current limit

void SmartMotorSetPowerExpanderStatusPort ( tVexAnalogPin port)

Set power expander status port.

Parameters

[in] port The analog port for status from the power expander

This allows the battery voltage of the power expander to be monitored. Monitoring occurs after the ptc in the power expander so can be used to cut power to those motors if necessary.

void SmartMotorPtcMonitorEnable ( void )

Enable current limit by monitoring the PTC temperatures.

void SmartMotorPtcMonitorDisable ( void )

Disable current limit by monitoring the PTC temperatures.

void SmartMotorCurrentMonitorEnable ( void )

Enable current limit by using a preset threshold.

Examples:: clawbot.c.

void SmartMotorCurrentMonitorDisable ( void )

Disable current limit by using a preset threshold.

void SmartMotorRun ( void )

Start the smart motor monitoring.

After initialization the smart motor tasks need to be started

Examples:: clawbot.c.

void SmartMotorStop ( void )

Stop smart motor tasks.

void SmartMotorDebugStatus ( void )

Dump controller and motor status to the debug stream.

void _SetMotor	(	int	index,
		int	value,
		bool_t	immediate,
			...
	)

Set motor to value.

Parameters

[in]	index	The motor index
[in]	value	The motor control value (speed)
[in]	immediate	If TRUE then bypass the slew rate control

Set a new control value for the motor. This is the only call that should be used to change motor speed. Using vexMotorSet will completely bugger up the code - do not use it

The naming of this function is inconsistent with the rest of the library to allow backwards compatibility with team 8888's existing library

void SmartMotorsInit ( void )

Initialize the smartMotor library.

Initialize the smart motor library - This function must be called once when the program starts. Motors are automatically detected and assigned to the cortex banls, if a power expander is used then it should be added by using the SmartMotorsAddPowerExtender function after calling SmartMotorsInit

Examples:: clawbot.c.

void SmartMotorLinkMotors	(	tVexMotor	master,
		tVexMotor	slave
	)

Link two motors.

Parameters

[in]	master	The master motor (usually with an encoder)
[in]	slave	The slave motor (usually un-encoded)

This lets us use encoder counts from one motor for another motor.

void SmartMotorsSetEncoderGearing	(	tVexMotor	index,
		float	ratio
	)

Set the encoder gearing.

Parameters

[in]	index	The motor index
[in]	ratio	The gear ratio

If using a quad encoder then it may not be directly connected to the motor. This function allows a change of gearing by changing the ticks per rpm. Only call this function once per motor after calling init

ratio is encoder revs/motor revs

void _SmartMotorsAddPowerExtender	(	int	p0,
		int	p1,
		int	p2,
		int	p3,
			...
	)

Add a power extender to the system.

Parameters

[in]	p0	The first power expander motor port
[in]	p1	The second power expander motor port
[in]	p2	The third power expander motor port
[in]	p3	The fourth power expander motor port

This function reassigns up to four motors to a power expander rather than ports on the cortex.

There is a macro SmartMotorsAddPowerExtender for ROBOTC compatibility

// the motor on port4 is assigned to the power expander
SmartMotorsAddPowerExtender( kVexMotor_4 );
// three motors are assigned to the power expander
SmartMotorsAddPowerExtender( ArmMotorA, ArmMotorB, ArmMotorC );

void _SmartMotorSetRpmSensor	(	tVexMotor	index,
		tVexAnalogPin	port,
		float	ticks_per_rev,
		bool_t	reversed,
			...
	)

Assign (non encoder) sensor for rpm measurement.

Parameters

[in]	index	The motor index
[in]	port	The sensor port
[in]	ticks_per_rev	ticks per rovolution for this sensor
[in]	reversed	A bool indicating motor direction is reversed

This function can be used to assign any sensor to a motor for calculating rpm, the most likely situation would be a potentiometer. The parameter ticks_per_rev is the sensor change for one revolution of the motor, for example, a potentiometer with 1:1 gearing has about 6000 ticks_per_rev, if the gearing was 7:1 then this would be reduced to about 857. Set reversed to true if the potentiometer value decreases with positive motor command values.

void SmartMotorSpeed	(	smartMotor *	m,
		int	deltaTime
	)

calculate speed in rpm for the motor

Parameters

[in]	m	Pointer to smartMotor structure
[in]	deltaTime	The time in mS from the last call to this function

Warning: Internal smartMotorLibrary function, do not call, ref only

void SmartMotorSimulateSpeed ( smartMotor * m)

calculate a simulated speed in rpm for a motor

Parameters

[in] m Pointer to smartMotor structure

Warning: Internal smartMotorLibrary function, do not call, ref only

Note: We needed to do something for motors which do not have encoders and are also not coupled to another motor and able to share its encoder

float SmartMotorCurrent	(	smartMotor *	m,
		float	v_battery
	)

Estimate smart motor current.

Parameters

[in]	m	Pointer to smartMotor structure
[in]	v_battery	The battery voltage in volts

Returns: The calculated current

Warning: Internal smartMotorLibrary function, do not call, ref only

Estimate current in Vex motor using vamfun's algorithm.
subroutine written by Vamfun...Mentor Vex 1508, 599.
7.13.2012 vamfu.nosp@m.n@ya.nosp@m.hoo.c.nosp@m.om... blog info http://vamfun.wordpress.com

Modified by James Pearman 7.28.2012.
Modified by James Pearman 10.1.2012 - more generalized code.

If cmd is positive then rpm must also be positive for this to work.

float SmartMotorControllerCurrent ( smartController * s)

Calculate the current for a controller bank.

Parameters

[in] s A pointer to a smartController structure

Returns: The calculated current

Warning: Internal smartMotorLibrary function, do not call, ref only

int SmartMotorSafeCommand	(	smartMotor *	m,
		float	v_battery
	)

Calculate safe current command.

Parameters

[in]	m	Pointer to smartMotor structure
[in]	v_battery	The battery voltage in volts

Returns: The calculated command value

Warning: Internal smartMotorLibrary function, do not call, ref only

Calculate a command for the motor which will result in a target current based on motor speed.

target_current should be set prior to calling the function and must be POSITIVE.

If command direction and rpm are not of the same polarity then this function tends to fallover due to back emf being of opposite direction to the drive direction. In this situation we have no choice but to let current go higher as the motor is changing directions and will in effect stall as it does through zero.

float SmartMotorTemperature	(	smartMotor *	m,
		int	deltaTime
	)

Calculate the PTC temperature for a motor.

Parameters

[in]	m	Pointer to smartMotor structure
[in]	deltaTime	The time in mS from the last call to this function

Returns: The calculated ptc temperature

Warning: Internal smartMotorLibrary function, do not call, ref only

Note: math by vamfun

float SmartMotorControllerTemperature	(	smartController *	s,
		int	deltaTime
	)

Calculate the PTC temperature for a controller bank.

Parameters

[in]	s	Pointer to smartController structure
[in]	deltaTime	The time in mS from the last call to this function

Returns: The calculated ptc temperature

Warning: Internal smartMotorLibrary function, do not call, ref only

Note: math by vamfun

void SmartMotorMonitorPtc	(	smartMotor *	m,
		float	v_battery
	)

Monitor Motor PTC temperature.

Parameters

[in]	m	Pointer to smartMotor structure
[in]	v_battery	The battery voltage in volts

Warning: Internal smartMotorLibrary function, do not call, ref only

If the temperature is above the set point then calculate a command that will result in a safe current

void SmartMotorControllerMonitorPtc	(	smartController *	s,
		float	v_battery
	)

Monitor controller bank PTC temperature.

Parameters

[in]	s	Pointer to smartController structure
[in]	v_battery	The battery voltage in volts

Warning: Internal smartMotorLibrary function, do not call, ref only

If the temperature is above the set point then calculate a commands for each motor that will result in a safe current.

void SmartMotorMonitorCurrent	(	smartMotor *	m,
		float	v_battery
	)

Monitor Motor Current.

Parameters

[in]	m	Pointer to smartMotor structure
[in]	v_battery	The battery voltage in volts

Warning: Internal smartMotorLibrary function, do not call, ref only

void SmartMotorControllerSetLed ( smartController * s)

Set the current monitor LED on or off.

Parameters

[in] s Pointer to smartController structure

Warning: Internal smartMotorLibrary function, do not call, ref only

Turn on status LED if either controller ptc is tripped or any connected motors are tripped. LED is only turned on in this function as it may be shared between more then one controller. The LED is briefly turned off each loop around the calling task but only remains off for a very short period which is inperceptable.

msg_t SmartMotorTask ( void * arg)

The smart motor task.

Parameters

[in] arg pointer to user data (not used)

Note: Running a little different in this version, instead of a 100mS delay and then calculations on each motor we do one motor each iteration

msg_t SmartMotorSlewRateTask ( void * arg)

The motor slew rate task.

Parameters

[in] arg pointer to user data (not used)

Task - compares the requested speed of each motor to the actual speed and increments or decrements to reduce the difference as necessary

Functions

Detailed Description

Function Documentation