uint16_t a2dReadMv(ADC_CHANNEL channel){
uint16_t val10 = a2dConvert10bit(channel); // 0..1023
uint16_t avcc = a2dGetAVcc();
return interpolateU(val10,0,1023, 0,avcc);
}
static void setSpeed(__ACTUATOR *actuator, DRIVE_SPEED speed){
SERVO* servo = (SERVO*)actuator;
DRIVE_SPEED current = servo->actuator.required_speed;
if(servo->actuator.inverted){
current *= -1;
}
// Can only cope with 21 servos
if(speed != current && servo->delay< 21){
uint8_t reg = (servo->delay * 3) + 1;
uint8_t msg[3];
struct s_servo_driver* driver = servo->driver;
const I2C_DEVICE* i2c = &(driver->i2cInfo);
// Get the pulse length in us
uint16_t pulse = interpolateU(speed, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX, servo->center_us - servo->range_us , servo->center_us + servo->range_us);
msg[0] = reg; // Register
msg[1] = (uint8_t)(pulse & 0xff); // lo
msg[2] = (uint8_t)(pulse >> 8); // hi
// The I2C address is always C2
i2cMasterSend(i2c, sizeof(msg), msg);
// rprintf("%u = %u\n",servo->delay,pulse);
}
// 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);
}
void sonyPS_setRumble(SONY_PS2* controller, uint8_t left, boolean right){
if(left || right){
// Turn on rumble on first use
if(!controller->rumble){
controller->rumble = TRUE;
sendCmd(controller,cmd_config,sizeof(cmd_config));
sendCmd(controller,cmd_rumble,sizeof(cmd_rumble));
sendCmd(controller,cmd_normal,sizeof(cmd_normal));
}
}
controller->rumbleRight=right;
controller->rumbleLeft=(left) ? interpolateU(left,0,0xff,0x40,0xff) : 0;
}
// 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);
}
// 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 void mux_setSpeed(__ACTUATOR *actuator, DRIVE_SPEED speed){
SERVO* servo = (SERVO*)actuator;
// Interpolate the values
uint16_t ticks=interpolateU(speed, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX, servo->min_ticks , servo->max_ticks);
// The min/max ticks are based on a 20ms frequency, but we actually using 2.5ms
// so multiply by 20/2.5 = 8
ticks *= 8; //
if(ticks!=servo->delay){
CRITICAL_SECTION{
servo->delay = ticks;
};
}
static void vgraph(DISPLAY* display,DISPLAY_COLUMN x,DISPLAY_COLUMN y, uint16_t pixels,uint16_t max,uint8_t height){
setVGraph(display);
pixels = interpolateU(pixels,0,max,0,8*height); // convert to number of pixels
while(height--){
_displayGoto(display,x,y+height);
uint8_t c;
c = (pixels>=8) ? 8 : pixels;
pixels -= c;
c = (c==0) ? ' ' : c-1;
_displaySendByte(display,c);
}
/*
* Inbuilt cmd uses both lines of the display
write(display,254);write(display,61); // Draw vertical graph
write(display,x+1); // 1s rel column
write(display,pixels); // The number of pixels
*/
}
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;
}
}
}
static void hgraph(DISPLAY* display, uint16_t pixels,uint16_t max,uint8_t width){
// Enable horiz graph mode
setHGraph(display);
pixels = interpolateU(pixels,0,max,0,5*width); // convert to number of pixels
while(width--){
uint8_t c;
c = (pixels>=5) ? 5 : pixels;
pixels -= c;
c = (c==0) ? ' ' : c-1;
_displaySendByte(display,c);
}
/*
write(display,254);write(display,124); // Draw horizontal graph
write(display,x+1); // 1s rel column
write(display,y+1); // 1s rel line
write(display,0); // From left to right
write(display,pixels); // The number of pixels
*/
}
// 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);
}
}
// Update the gait runner and move servos to new positions
// Call it from your main loop or via the scheduler to do it in the background
// NB There is no point scheduling any faster than 20ms as that is the servo refresh rate
// Return true if an animation is playing
boolean gaitRunnerProcess(G8_RUNNER* runner){
if(!gaitRunnerIsPlaying(runner) || runner->speeds==null){
return FALSE;
}
TICK_COUNT now = clockGetus();
int16_t interval = (now - runner->startTime)>>16;
if(interval == 0){
return TRUE;
}
// There has been a noticeable change in time
runner->startTime = now;
if(runner->backwards){
interval *= -1;
}
interval *= runner->speed;
// Re-check as drive speed could be zero
if(interval == 0){
return TRUE;
}
// Locate the current animation
const G8_ANIMATION* animation = &runner->animations[runner->animation];
// Update the current time with the new interval
int16_t currentTime = runner->currentTime + interval;
if(currentTime >= runner->totalTime){
// We have finished playing the animation
if(pgm_read_byte(&animation->sweep)==FALSE){
currentTime %= runner->totalTime; // Set back to start of loop
if(runner->repeatCount){
runner->repeatCount -= 1; // One less frame to go
if(runner->repeatCount==0){
runner->playing = FALSE; // we have reached the end
currentTime = 0; // set servos to final position
}
}
}else{
// Start going backwards through the animation
currentTime = runner->totalTime - (currentTime - runner->totalTime);
runner->backwards = TRUE;
}
}else if(currentTime < 0){
// We have moved before the start
if(pgm_read_byte(&animation->sweep)==FALSE){
currentTime = runner->totalTime + currentTime;
if(runner->repeatCount){
runner->repeatCount += 1; // One more frame to go
if(runner->repeatCount==0){
runner->playing = FALSE; // we have reached the end
currentTime = 0; // set servos to start position
}
}
}else{
// We have completed a sweep
runner->backwards = FALSE;
currentTime = -currentTime;
if(runner->repeatCount){
runner->repeatCount -= 1; // One less frame to go
if(runner->repeatCount==0){
runner->playing = FALSE; // we have reached the end
currentTime = 0; // set servos to initial position
}
}
}
}
runner->currentTime = currentTime; // range is 0....totalTime
// Current time in the range 0...SCALE_X
uint16_t frameTime = interpolateU(currentTime, 0,runner->totalTime, 0, SCALE_X);
uint16_t frameStartTime = 0;
uint16_t frameEndTime = SCALE_X;
// Locate the correct frame
const G8_FRAME* frame = (const G8_FRAME*)pgm_read_word(&animation->frames);
uint8_t i;
for(i = pgm_read_byte(&animation->numFrames)-1; i>0; i--){
const G8_FRAME* f = &frame[i];
frameStartTime = pgm_read_word(&f->time);
if(frameStartTime <= frameTime){
frame = f;
break;
}
frameEndTime = frameStartTime;
frameStartTime = 0;
}
runner->frame = i;
#ifdef DEBUG
rprintf("\n%u,%d",i,currentTime);
#endif
// Now have:- frameStartTime <= frameTime <= frameEndTime
// We now need to find the distance along the curve (0...1) that represents
// the x value = frameTime
// First guess from 0..1
uint16_t frameTimeOffset = frameTime-frameStartTime;
double distanceGuess = ((double)(frameTimeOffset)) / ((double)(frameEndTime-frameStartTime));
const G8_LIMB_POSITION* limb = (const G8_LIMB_POSITION*)pgm_read_word(&frame->limbs);
for(uint16_t l = 0; l < runner->num_actuators; l++, limb++){
double distanceMin = 0.0;
double distanceMax = 1.0;
double distance = distanceGuess;
// Find the correct distance along the line for the required frameTime
for(uint8_t iterations=0; iterations<20; iterations++){
uint16_t actualX = calcX(limb, distance);
if(actualX == frameTimeOffset) break; // Found it
if( actualX < frameTimeOffset){
// We need to increase t
distanceMin = distance;
}else{
distanceMax = distance;
}
// Next guess is half way between
distance = distanceMin + ((distanceMax - distanceMin) / 2);
}
// We now know the distance
runner->speeds[l] = calcY(limb,distance);
#ifdef DEBUG
rprintf(",%d",speed);
#endif
} // next limb
#ifndef DEBUG
// Set all the servo speeds in quick succession
for(uint16_t l = 0; l < runner->num_actuators; l++){
__ACTUATOR* servo = (__ACTUATOR*)pgm_read_word(&runner->actuators[l]);
int16_t speed = (int16_t)(runner->speeds[l]) + (int16_t)(runner->delta[l]);
speed = CLAMP(speed,DRIVE_SPEED_MIN,DRIVE_SPEED_MAX);
__act_setSpeed(servo,(DRIVE_SPEED)speed);
}
#endif
return gaitRunnerIsPlaying(runner);
}
uint16_t __phidgetsRead(ADC_CHANNEL adcPin){
uint16_t raw = a2dConvert10bit(adcPin);
return interpolateU(raw, 0, 1023, 0, 1000);
}
