/*
* Test the problematic degrees with the arc angles
* being 0' 90' 180' 270'
*/
void EM411TestTan(void)
{
float angle=0.0;
// 0'
angle = EM411Tan(0, 1);
UARTPrintf("Angle should 0' result %f'\r\n", angle/(360*2*M_PI));
// 45'
angle = EM411Tan(1, 1);
UARTPrintf("Angle should 45' result %f'\r\n", angle/(2*M_PI)*360);
// 90'
angle = EM411Tan(1, 0);
UARTPrintf("Angle should 90' result %f'\r\n", angle/(2*M_PI)*360);
// 135'
angle = EM411Tan(1, -1);
UARTPrintf("Angle should 135' result %f'\r\n", angle/(2*M_PI)*360);
// 180'
angle = EM411Tan(0, -1);
UARTPrintf("Angle should 180' result %f'\r\n", angle/(2*M_PI)*360);
// 225'
angle = EM411Tan(-1, -1);
UARTPrintf("Angle should 225' result %f'\r\n", angle/(2*M_PI)*360);
// 270'
angle = EM411Tan(-1, 0);
UARTPrintf("Angle should 270' result %f'\r\n", angle/(2*M_PI)*360);
// 315'
angle = EM411Tan(-1, 1);
UARTPrintf("Angle should 315' result %f'\r\n", angle/(2*M_PI)*360);
/*
* Sleep to show the slow humans the results.
*/
chThdSleepMilliseconds(100000);
}
// This function pings all of the possible brushless motor controller addresses
// by sending a 0 command. A response indicates that a controller (and hopefully
// also a motor) is present. The contents of the response indicate the type and
// features of the controller.
void DetectMotors(void)
{
// TODO: if (motors_on) return;
// Send a 0 command to each brushless controller address and record any
// responses.
uint8_t motors = 0; // Bit field representing motors present.
uint8_t setpoint = 0; // Do not command the motors to move
enum BLCStatusCode blc_status_code = BLC_STATUS_UNKNOWN;
for (uint8_t i = 0; i < MOTORS_MAX; i++)
{
I2CTxThenRx(MOTORS_BASE_ADDRESS + (i << 1), &setpoint, sizeof(setpoint),
(volatile uint8_t *)&blc_status_[i], sizeof(struct BLCStatus));
I2CWaitUntilCompletion();
// I2C will give an error if there is no response.
if (!I2CError())
{
motors |= (1 << i); // Mark this motor as present
// Check that all controllers are the same type.
if (blc_status_code == BLC_STATUS_UNKNOWN)
blc_status_code = blc_status_[i].status_code;
else if (blc_status_[i].status_code != blc_status_code)
blc_error_bits_ |= BLC_ERROR_BITS_INCONSISTENT_SETTINGS;
}
}
// Identify additional features of the brushless controllers.
UARTPrintf("Detected motor controllers with the following compatibility:");
switch (blc_status_code)
{
case BLC_STATUS_V3_FAST_READY:
UARTPrintf(" fast mode (20 kHz PWM)");
blc_feature_bits_ |= BLC_FEATURE_20KHz;
case BLC_STATUS_V3_READY:
UARTPrintf(" version 3");
blc_feature_bits_ |= BLC_FEATURE_V3;
case BLC_STATUS_V2_READY:
UARTPrintf(" version 2");
blc_feature_bits_ |= BLC_FEATURE_EXTENDED_COMMS;
setpoint_length_ = sizeof(uint16_t);
break;
default:
UARTPrintf(" version 1");
break;
}
// Check for missing or extra motors. Assumes that present motors have
// contiguous addresses beginning with 0.
n_motors_ = eeprom_read_byte(&eeprom.n_motors);
if (((1 << n_motors_) - 1) & !motors)
blc_error_bits_ |= BLC_ERROR_BITS_MISSING_MOTOR;
if (motors & !((1 << n_motors_) - 1))
blc_error_bits_ |= BLC_ERROR_BITS_EXTRA_MOTOR;
if (blc_error_bits_ & ~_BV(BLC_ERROR_BITS_INCONSISTENT_SETTINGS))
{
UARTPrintf("ERROR: expected motor controllers with addresses: 0 - %i",
n_motors_ - 1);
UARTPrintf(" Bit field of responding addresses is: %X", motors);
}
}