static void setConnected(__ACTUATOR *actuator, boolean connected){
TOSHIBA_TB6612FNG_2pin_MOTOR* motor = (TOSHIBA_TB6612FNG_2pin_MOTOR*)actuator;
const TimerCompare* channel1 = compareFromIOPin(motor->pwm1);
const TimerCompare* channel2 = compareFromIOPin(motor->pwm2);
if(connected){
// connect
// restore previous speed
setSpeed(actuator, act_getSpeed(motor));
}else{
// Set both outputs to low to coast - by setting duty cycle to TOP
compareSetThreshold(channel1, timerGetTOP(compareGetTimer(channel1)));
compareSetThreshold(channel2, timerGetTOP(compareGetTimer(channel2)));
}
}
static void sound(uint8_t b){
b = (b & 15);
#ifdef _LOG_
loggerSnd((char)(b+'0'));
#endif
#if !defined(_WINDOWS_)
// Update PWM volume
if(channel){
compareSetThreshold(channel,Volume[b]);
}
#endif
}
// Call back - for when the speed has been set
static void setSpeed(__ACTUATOR *actuator, DRIVE_SPEED speed){
MOTOR* motor = (MOTOR*)actuator;
const TimerCompare* channel = compareFromIOPin(motor->pwm);
const Timer* timer = compareGetTimer(channel);
uint16_t top = timerGetTOP(timer);
// New compare threshold
uint16_t delay=0;
if( speed > 0 ){
delay = interpolateU(speed, 0, DRIVE_SPEED_MAX, 0 , top);
if(motor->direction2==null){
// one wire so delay = top - delay
delay = top - delay;
}
// Set direction1 high, direction2 low (if there is one)
pin_make_output(motor->direction1,TRUE);
pin_make_output(motor->direction2,FALSE);
}else if(speed < 0){
delay = interpolateU(speed, 0, DRIVE_SPEED_MIN, 0 , top);
// Set direction1 low, direction2 high (if there is one)
pin_make_output(motor->direction1,FALSE);
pin_make_output(motor->direction2,TRUE);
}else{
// brake
if(motor->direction2){
// There are two direction pins - so set both to same value
pin_make_output(motor->direction1,FALSE);
pin_make_output(motor->direction2,FALSE);
delay = top; // full speed brake
}else{
// Only has one direction pin
// Set direction1 low
pin_make_output(motor->direction1,FALSE);
// PWM delay = 0 so pwm = low
// ie both low = brake
}
}
// Change the duty cycle
compareSetThreshold(channel,delay);
}
// Set the duty cycle
void pwmSetDutyCycle(const IOPin* pin, PERCENTAGE duty){
const TimerCompare* channel = compareFromIOPin(pin);
if(channel){
const Timer* timer = compareGetTimer(channel);
uint32_t top = timerGetTOP(timer);
// Limit the duty cycle
if(duty>100)
duty=100;
// 100 => top
// duty => x
// x = (top * duty) / 100
// top *= duty;
// top /= 100;
// uint16_t delay = top;
uint16_t delay = interpolateU(duty, 0,100, 0,top);
// Change the duty cycle
compareSetThreshold(channel,delay);
}
}
// Call back - for when the speed has been set
static void setSpeed(__ACTUATOR *actuator, DRIVE_SPEED speed){
MOTOR* motor = (MOTOR*)actuator;
const TimerCompare* channel = compareFromIOPin(motor->pwm);
const Timer* timer = compareGetTimer(channel);
uint16_t top = timerGetTOP(timer);
// New compare threshold
uint16_t delay=0;
if( speed > 0 ){
delay = interpolateU(speed, 0, DRIVE_SPEED_MAX, 0 , top);
// Set direction1 high, direction2 low
pin_make_output(motor->direction1,TRUE);
pin_make_output(motor->direction2,FALSE);
}else if(speed < 0){
delay = interpolateU(speed, 0, DRIVE_SPEED_MIN, 0 , top);
// Set direction1 low, direction2 high low
pin_make_output(motor->direction1,FALSE);
pin_make_output(motor->direction2,TRUE);
}else{
// brake
if(motor->direction2){
// There are two direction pins - so set both to same value
pin_make_output(motor->direction1,FALSE);
pin_make_output(motor->direction2,FALSE);
}else{
// Only has one direction pin
// Set direction1 to an input with no pullup ie disconnect
pin_make_input(motor->direction1,FALSE);
}
}
// Change the duty cycle
compareSetThreshold(channel,delay);
}
static void uartswTxBitService(const TimerCompare *channel, void* _uart)
{
SW_UART* uart = (SW_UART*) _uart;
if(uart->txBitNum)
{
// there are bits still waiting to be transmitted
if(uart->txBitNum > 1)
{
// transmit data bits (inverted, LSB first)
if( (uart->txData & 0x01) )
pin_high(uart->_uart_.tx_pin);
else
pin_low(uart->_uart_.tx_pin);
// shift bits down
uart->txData >>= 1;
}
else
{
// transmit stop bit
if(uart->inverted){
pin_low(uart->_uart_.tx_pin);
}else{
pin_high(uart->_uart_.tx_pin);
}
}
// schedule the next bit
uint16_t top = 1 + timerGetTOP(compareGetTimer(channel));
uint16_t next = (compareGetThreshold(channel) + uart->dataBitLength);
if(next >= top){
next -= top;
}
compareSetThreshold(channel, next );
// count down
uart->txBitNum--;
}
// Callback - for when the speed has been set
static void setSpeed(__ACTUATOR *actuator, DRIVE_SPEED speed){
TOSHIBA_TB6612FNG_2pin_MOTOR* motor = (TOSHIBA_TB6612FNG_2pin_MOTOR*)actuator;
const TimerCompare* channel1 = compareFromIOPin(motor->pwm1);
const TimerCompare* channel2 = compareFromIOPin(motor->pwm2);
// New compare threshold
uint16_t delay=0;
if( speed > 0 ){
delay = interpolateU(speed, 0, DRIVE_SPEED_MAX, 0, timerGetTOP(compareGetTimer(channel2)));
compareSetThreshold(channel1,0); // Keep permanently high
compareSetThreshold(channel2,delay); // pwm channel 2
}else if(speed < 0){
delay = interpolateU(speed, 0, DRIVE_SPEED_MIN, 0 , timerGetTOP(compareGetTimer(channel1)));
compareSetThreshold(channel2,0); // Keep permanently high
compareSetThreshold(channel1,delay); // pwm channel 1
}else{
// brake
// Set both pins high
compareSetThreshold(channel1,0); // Keep permanently high
compareSetThreshold(channel2,0); // Keep permanently high
}
}
