static void setRanging( boolean r, SERVO_LIST* const servos, uint8_t numServos){
if(ranging != r){
ranging = r;
DRIVE_SPEED speed = (ranging) ? DRIVE_SPEED_MAX : DRIVE_SPEED_BRAKE;
for(int i=0; i<numServos; i++){
SERVO* servo = (SERVO*)pgm_read_word(&servos[i]);
act_setSpeed(servo,speed);
}
}
}
static void setConnected(__ACTUATOR *actuator, boolean connected){
SABERTOOTH_MOTOR* remote = (SABERTOOTH_MOTOR*)actuator;
if(connected){
DRIVE_SPEED speed = act_getSpeed(remote);
actuator->required_speed=-128;
act_setSpeed(remote, speed);
}else{
// Set speed = 0 in forwards to coast
__saberOutput(remote, TRUE, 0);
}
}
void servoSetConfig(SERVO* servo, uint16_t center, uint16_t range){
servo->center_us = center; // Set the new center value
servo->range_us = range; // Set the new range value
SERVO_DRIVER* driver = servo->driver; // Get the current driver
if(driver){
// The servo is 'live'
CRITICAL_SECTION_START;
// Take the useable servo range and calculate the min/max pulses
// (SERVO_CYCLE*1000) = servo_cycle
// min x
// x = (min * g_servo_cycle)/(SERVO_CYCLE*1000)
uint32_t min = center - range;
uint32_t m1 = servo->top / 1000;
uint32_t m2 = m1 * min;
uint32_t m3 = m2 / SERVO_CYCLE;
if( (m3 & 0xFFFF0000UL)!=0){
// doesn't fit
setError(SERVO_TIMING);
m3 = 0xFFFFUL;
}
servo->min_ticks = m3;
uint32_t max = center + range;
m2 = m1 * max;
m3 = m2 / SERVO_CYCLE;
if( (m3 & 0xFFFF0000UL)!=0){
// doesn't fit
setError(SERVO_TIMING);
m3 = 0xFFFFUL;
}
servo->max_ticks = m3;
// Set the speed again
if(act_isConnected(servo)){
DRIVE_SPEED speed = servo->actuator.required_speed;
servo->actuator.required_speed-=1;
act_setSpeed(servo,speed);
}
CRITICAL_SECTION_END;
}
}
// Pass the list of servos, the list should be in PROGMEM space to save Flash RAM
// The specified Timer must implement timer compare interrupts and, if so, it will
// ise the timer compare channel A (if there is more than one)
void toshibaTB6612FNG_2pin_Init(TOSHIBA_TB6612FNG_2pin_MOTOR_DRIVER* driver, uint32_t freq){
uint32_t deciHertz = 10 * freq;
// Make sure each servo is set as an output
for(int i= driver->num_motors-1;i>=0;i--){
TOSHIBA_TB6612FNG_2pin_MOTOR* motor = (TOSHIBA_TB6612FNG_2pin_MOTOR*)pgm_read_word(&driver->motors[i]);
if(initPWM(motor->pwm1,deciHertz)){
if(initPWM(motor->pwm2, deciHertz)){
// Connect motor to driver
motor->actuator.class = &c_motors;
}
}
// Start off braking
act_setSpeed(motor,DRIVE_SPEED_BRAKE);
// Indicate the motor is connected
act_setConnected(motor,TRUE);
}
// Pass the list of motors, the list should be in PROGMEM space to save Flash RAM
// The specified Timer must implement timer compare interrupts and, if so, it will
// use the timer compare channel A (if there is more than one)
void motorL293Init(MOTOR_DRIVER* driver, uint32_t freq){
// Make sure each servo is set as an output
for(int i= driver->num_motors-1;i>=0;i--){
MOTOR* motor = (MOTOR*)pgm_read_word(&driver->motors[i]);
// Connect motor to driver
motor->actuator.sclass = &c_l293;
// Make sure the motor pins are set as output pins
pin_make_output(motor->pwm, FALSE);
pin_make_output(motor->direction1, FALSE);
pin_make_output(motor->direction2, FALSE);
// Start off braking
act_setSpeed(motor,DRIVE_SPEED_BRAKE);
// Indicate the motor is connected
act_setConnected(motor,TRUE);
}
}
void sabertoothInit(SABERTOOTH_DRIVER* driver){
switch(driver->mode){
case PACKETIZED:{
// Pause for 2s from power on
delay_ms(2000);
// Set baud rate
_uartInit(driver->uart,driver->baudRate);
// send bauding character
_uartSendByte(driver->uart,0xaa);
delay_ms(20);
_uartSendByte(driver->uart,0xaa);
delay_ms(20);
_uartSendByte(driver->uart,0xaa);
delay_ms(20);
_uartSendByte(driver->uart,0xaa);
delay_ms(20);
_uartSendByte(driver->uart,0xaa);
delay_ms(20);
}
break;
case SIMPLE:{
_uartInit(driver->uart,driver->baudRate);
}
break;
}
// set each motor to stop
for(int i=0;i<driver->numMotors;i++){
SABERTOOTH_MOTOR* motor = (SABERTOOTH_MOTOR*)pgm_read_word(&driver->motors[i]);
motor->actuator.sclass=&c_Sabertooth;
motor->driver = driver;
act_setSpeed(motor,0);
act_setConnected(motor,TRUE);
}
}
void servosSetSpeed(const SERVO_DRIVER* driver, DRIVE_SPEED speed){
for(int i=driver->num_servos - 1; i >= 0; i--){
SERVO* servo = (SERVO*)pgm_read_word(&driver->servos[i]);
act_setSpeed(servo, speed);
}
}
// This routine is called once to allow you to set up any other variables in your program
// You can use 'clock' function here.
// The loopStart parameter has the current clock value in μS
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
act_setSpeed(&servo_0,DRIVE_SPEED_CENTER);
act_setSpeed(&servo_1,DRIVE_SPEED_CENTER);
act_setSpeed(&servo_2,DRIVE_SPEED_CENTER);
act_setSpeed(&servo_3,DRIVE_SPEED_CENTER);
act_setSpeed(&servo_4,DRIVE_SPEED_CENTER);
act_setSpeed(&servo_5,DRIVE_SPEED_CENTER);
// starting values for trims.
data[RX_SERVO_0_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_0_CH]);
data[RX_SERVO_0_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_0_CL]);
data[RX_SERVO_1_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_1_CH]);
data[RX_SERVO_1_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_1_CL]);
data[RX_SERVO_2_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_2_CH]);
data[RX_SERVO_2_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_2_CL]);
data[RX_SERVO_3_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_3_CH]);
data[RX_SERVO_3_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_3_CL]);
data[RX_SERVO_4_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_4_CH]);
data[RX_SERVO_4_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_4_CL]);
data[RX_SERVO_0_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_0_MUL]);
data[RX_SERVO_1_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_1_MUL]);
data[RX_SERVO_2_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_2_MUL]);
data[RX_SERVO_3_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_3_MUL]);
data[RX_SERVO_4_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_4_MUL]);
data[RX_AUTOP_X_MUL] = eeprom_read_byte(&data_eeprom[RX_AUTOP_X_MUL]);
data[RX_AUTOP_Y_MUL] = eeprom_read_byte(&data_eeprom[RX_AUTOP_Y_MUL]);
data[RX_AUTOP_X_TRIM] = eeprom_read_byte(&data_eeprom[RX_AUTOP_X_TRIM]);
data[RX_AUTOP_Y_TRIM] = eeprom_read_byte(&data_eeprom[RX_AUTOP_Y_TRIM]);
data[RX_GYRO_X_OFFSET_L] = eeprom_read_byte(&data_eeprom[RX_GYRO_X_OFFSET_L]);
data[RX_GYRO_X_OFFSET_H] = eeprom_read_byte(&data_eeprom[RX_GYRO_X_OFFSET_H]);
data[RX_GYRO_Y_OFFSET_L] = eeprom_read_byte(&data_eeprom[RX_GYRO_Y_OFFSET_L]);
data[RX_GYRO_Y_OFFSET_H] = eeprom_read_byte(&data_eeprom[RX_GYRO_Y_OFFSET_H]);
if ((data[RX_SERVO_1_CH] < 0x60) | (data[RX_SERVO_1_CH] > 0x80)){
reset_trims();
}
if ((data[RX_SERVO_3_MUL] > 137) || (data[RX_SERVO_3_MUL] < 117)){ data[RX_SERVO_3_MUL] = 127;}
if ((data[RX_SERVO_4_MUL] > 137) || (data[RX_SERVO_4_MUL] < 117)){ data[RX_SERVO_4_MUL] = 127;}
// initialise RF module.
cyrf6936_Initialise_soft(&cyrf_0);
cyrf6936_Initialise_soft(&cyrf_1);
// enable interrupt on pin connected to cyrf6936 interrupt pin.
cyrfIntEnable();
frameStart=clockGetus();
data[RX_BAT_VOLT] = a2dConvert8bit(ADC_CH_ADC0);
AtEst1.AYZ = AtEst0.AYZ = 0x2A7FF;
AtEst1.AXZ = AtEst1.AXZ = 0x38DFF;
return 0; // don't pause after
}
