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)));
}
}
// Set up PWM on the given pin
boolean pwmInitDeciHertz(const IOPin* pin, uint32_t deciHertz, PERCENTAGE duty, uint32_t* actualDeciHertz){
boolean rtn = FALSE;
const TimerCompare* channel = compareFromIOPin(pin);
if(channel==null){
setError(PWM_PIN_NOT_AVAILABLE);
}else{
// The pin is valid
// The pin is valid and available
TIMER_MODE mode;
uint16_t icr;
uint16_t prescaler;
uint32_t actual;
const Timer* timer = compareGetTimer(channel);
// Find the best PWM setting
boolean valid = timerCalcPwm(timer, deciHertz, 100, &mode, &icr, &prescaler, &actual);
if(!valid){
// There is no PWM setting that is valid
setError( (timerIsInUse(timer)) ? PWM_TIMER_IN_USE : TIMER_HAS_NO_PWM );
}else{
// Lets set up the PWM
timerSetMode(timer,mode);
if(modeIsICR(mode)){
// Set the ICR
PORT icrPort = pgm_read_word(&timer->pgm_icr);
_SFR_MEM16(icrPort)=icr;
}
// Mark the channel as in use
// compareAttach(channel,&nullTimerCompareCallback,0,null);
// Turn the pin into an output, low
pin_make_output(pin, FALSE);
// Turn on the PWM pin output
compareSetOutputMode(channel, CHANNEL_MODE_NON_INVERTING);
// Turn on the timer
timerSetPrescaler(timer,prescaler);
// Set the initial duty cycle
pwmSetDutyCycle(pin,duty);
// Set the return value
if(actualDeciHertz){
*actualDeciHertz = actual;
}
rtn = TRUE;
}
}
return rtn;
}
// 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
}
}
static boolean initPWM(const IOPin* pin, uint32_t deciHertz){
const TimerCompare* channel = compareFromIOPin(pin);
if(channel==null){
setError(PWM_PIN_NOT_AVAILABLE);
return FALSE;
}
if(compareIsInUse(channel)){
setError(PWM_PIN_IN_USE);
return FALSE;
}
TIMER_MODE mode;
uint16_t icr;
uint16_t prescaler;
const Timer* timer = compareGetTimer(channel);
// Find the best PWM setting for 10kHz, with 128 steps
boolean valid = timerCalcPwm(timer, deciHertz, 128, &mode, &icr, &prescaler);
if(!valid){
// There is no PWM setting that is valid
setError( (timerIsInUse(timer)) ? PWM_TIMER_IN_USE : TIMER_HAS_NO_PWM );
}else{
// Lets set up the PWM
if(!timerIsInUse(timer)){
timerSetMode(timer,mode);
if(modeIsICR(mode)){
// Set the ICR
PORT icrPort = pgm_read_word(&timer->pgm_icr);
_SFR_MEM16(icrPort)=icr;
}
}
// Make it an output pin and set high for brake
pin_make_output(pin,TRUE);
// Use inverting PWM
compareSetOutputMode(channel,CHANNEL_MODE_INVERTING);
// Mark the channels as in use
compareAttach(channel,&nullTimerCompareCallback,0,null);
// Do this last as it then turns on the timer
timerSetPrescaler(timer,prescaler);
}
return valid;
}
// 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);
}
PERCENTAGE pwmGetDutyCycle(const IOPin* pin){
PERCENTAGE rtn = 0;
const TimerCompare* channel = compareFromIOPin(pin);
if(channel){
const Timer* timer = compareGetTimer(channel);
uint32_t top = timerGetTOP(timer);
uint32_t duty = compareGetThreshold(channel);
// top => 100
// duty => x
// x = (100 * duty) / top
duty *= 100;
duty /= top;
rtn = duty;
}
return rtn;
}
void speechInit(const IOPin* pin){
pwmPin = pin;
timeFactor = roundup(cpu_speed,550000UL);
// Use 20kHz PWM on the pin
pwmInitHertz(pin,20000,50,null);
// Find the compare values for volume levels 0-15
channel = compareFromIOPin(pin);
if(channel){
const Timer* timer = compareGetTimer(channel);
uint32_t top = timerGetTOP(timer)-1;
for(int8_t v =0; v<16; v++){
uint16_t delay = interpolateU(v, 0,15, 0,top);
Volume[v] = 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--;
}