static void cmd_adjust(BaseSequentialStream *chp, int argc, char *argv[]) {
float gyrodata[2][3];
(void) argc;
(void) argv;
chprintf(chp, "Adjust the motorcycle vertically and press the User button\r\n");
while (!palReadPad(GPIOA, GPIOA_BUTTON)) { }
if (!readGyro(gyrodata[0])) {
chprintf(chp, "Error getting gyrodata!\r\n");
return;
}
else {
chprintf(chp, "Got gyrodata for the 1st time!\r\n");
}
chThdSleepMilliseconds(5000);
chprintf(chp, "Now place it on the side stand and press the button again\r\n");
while (!palReadPad(GPIOA, GPIOA_BUTTON)) { }
if (!readGyro(gyrodata[1])) {
chprintf(chp, "Error getting gyrodata!\r\n");
return;
} else {
chprintf(chp, "Got gyrodata for the 2nd time!\r\n");
}
chprintf(chp, "Got your gyrodata successfully\r\n");
uint8_t i = 0;
for (i = 0; i < 2; i++) {
chprintf(chp, "%d\t %f\t %f\t %f\r\n", i, gyrodata[i][0], gyrodata[i][1], gyrodata[i][2]);
}
}
void computeGyroRTBias(void)
{
uint8_t axis;
uint16_t samples;
float gyroSum[3] = { 0.0f, 0.0f, 0.0f };
gyroCalibrating = true;
for (samples = 0; samples < 2000; samples++) {
readGyro();
computeGyroTCBias();
gyroSum[ROLL] += rawGyro[ROLL].value - gyroTCBias[ROLL];
gyroSum[PITCH] += rawGyro[PITCH].value - gyroTCBias[PITCH];
gyroSum[YAW] += rawGyro[YAW].value - gyroTCBias[YAW];
delayMicroseconds(1000);
}
for (axis = ROLL; axis < 3; axis++) {
gyroRTBias[axis] = (float) gyroSum[axis] / 2000.0f;
}
gyroCalibrating = false;
}
/***************************************************************************
PUBLIC FUNCTIONS
***************************************************************************/
void Adafruit_LSM9DS0::read()
{
/* Read all the sensors. */
readAccel();
readMag();
readGyro();
readTemp();
}
/* * * * * * * * * *
* Systick Handler *
* * * * * * * * * */
void systick()
{
if (b_useIR)
{
readSensor();
}
if (b_useGyro)
{
readGyro();
}
if (b_useSpeedProfile)
{
speedProfile();
}
}
void MPU6000::init() {
// Reset device.
txbuf[0] = mpu6000::REG_PWR_MGMT_1;
txbuf[1] = 0x80;
exchange(2);
chThdSleepMilliseconds(100); // TODO(yoos): Check whether or not datasheet specifies this holdoff.
// Set sample rate to 1 kHz.
txbuf[0] = mpu6000::REG_SMPLRT_DIV;
txbuf[1] = 0x00;
exchange(2);
// Set DLPF to 4 (20 Hz gyro bandwidth1 Hz accelerometer bandwidth)
txbuf[0] = mpu6000::REG_CONFIG;
txbuf[1] = 0x04;
exchange(2);
// Wake up device and set clock source to Gyro Z.
txbuf[0] = mpu6000::REG_PWR_MGMT_1;
txbuf[1] = 0x03;
exchange(2);
// Set gyro full range to 2000 dps. We first read in the current register
// value so we can change what we need and leave everything else alone.
// See DS p. 12.
txbuf[0] = mpu6000::REG_GYRO_CONFIG | (1<<7);
exchange(2);
chThdSleepMicroseconds(0); // TODO(yoos): Without this, the GYRO_CONFIG register does not get set. This was not the case in the old C firmware. Why?
txbuf[0] = mpu6000::REG_GYRO_CONFIG;
txbuf[1] = (rxbuf[1] & ~0x18) | 0x18;
exchange(2);
// Set accelerometer full range to 16 g. See DS p. 13.
txbuf[0] = mpu6000::REG_ACCEL_CONFIG | (1<<7);
exchange(2);
txbuf[0] = mpu6000::REG_ACCEL_CONFIG;
txbuf[1] = (rxbuf[1] & ~0x18) | 0x18;
exchange(2);
// Read once to clear out bad data?
readGyro();
readAccel();
}
static void cmd_gyrodata(BaseSequentialStream *chp, int argc, char *argv[]) {
float gyrodata[3];
uint32_t times = 5;
uint32_t delay = 200;
if (argc >= 1) {
times = atoi(argv[0]);
}
if (argc >= 2) {
delay = atoi(argv[1]);
}
uint32_t i = 0;
chprintf(chp, " Number 1\t Number 2\t Number 3\t #\r\n");
for (i = 0; i < times; i++) {
chThdSleepMilliseconds(delay);
if (readGyro(gyrodata)) {
chprintf(chp, " %f\t %f\t %f\t %d\r\n", gyrodata[0], gyrodata[1], gyrodata[2], i);
}
}
}
void Gyroscope::calibrateStep() {
// Don't continue to average if acceptable offsets have already been computed.
if (calibrated()) {
return;
}
GyroscopeReading reading = readGyro();
for (std::size_t i = 0; i < 3; i++) {
offsets[i] = (offsets[i] * calibrationCount + reading.axes[i]) /
(calibrationCount + 1);
}
calibrationCount++;
if (std::abs(reading.axes[0] > 0.1) || std::abs(reading.axes[1] > 0.1) ||
std::abs(reading.axes[2] > 0.1)) {
calibrationCount = 0;
}
}
// This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average
// them, scales them to gs and deg/s, respectively, and then passes the biases to the main sketch
// for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store
// the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to
// subtract the biases ourselves. This results in a more accurate measurement in general and can
// remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner
// is good practice.
void LSM9DS1::calibrate(bool autoCalc)
{
uint8_t data[6] = {0, 0, 0, 0, 0, 0};
uint8_t samples = 0;
int ii;
int32_t aBiasRawTemp[3] = {0, 0, 0};
int32_t gBiasRawTemp[3] = {0, 0, 0};
// Turn on FIFO and set threshold to 32 samples
enableFIFO(true);
setFIFO(FIFO_THS, 0x1F);
while (samples < 0x1F)
{
samples = (xgReadByte(FIFO_SRC) & 0x3F); // Read number of stored samples
}
for(ii = 0; ii < samples ; ii++)
{ // Read the gyro data stored in the FIFO
readGyro();
gBiasRawTemp[0] += gx;
gBiasRawTemp[1] += gy;
gBiasRawTemp[2] += gz;
readAccel();
aBiasRawTemp[0] += ax;
aBiasRawTemp[1] += ay;
aBiasRawTemp[2] += az - (int16_t)(1./aRes); // Assumes sensor facing up!
}
for (ii = 0; ii < 3; ii++)
{
gBiasRaw[ii] = gBiasRawTemp[ii] / samples;
gBias[ii] = calcGyro(gBiasRaw[ii]);
aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
aBias[ii] = calcAccel(aBiasRaw[ii]);
}
enableFIFO(false);
setFIFO(FIFO_OFF, 0x00);
if (autoCalc) _autoCalc = true;
}
void MAIN(void) {
Quaternion lage(1, 0,0,0);
Vector3D vx(1,0,0), v;
double giroX, giroY, giroZ;
Quaternion deltaLage;
YPR ypr;
for(int i = 0; i <100; i++) { // shall be: while(1)
ypr = lage.toYPR();
v = vx.qRotate(lage);
PRINTF("------------ Step %d\n", i);
PRINTF("Currten attitude quat:");
lage.print();
PRINTF("Currten attitude v :");
v.print();
PRINTF("Currten attitude ypr :");
ypr.print();
ypr = lage.toYPRnils();
PRINTF("Currten attitude yprN:");
ypr.print();
readGyro(giroX, giroY, giroZ);
ypr = YPR(giroX, giroY, giroZ);
PRINTF("gyro-ypr :\t\t ");
ypr.print();
deltaLage = ypr.toQuaternion().normalize();
PRINTF("gyro-quat:");
deltaLage.print();
lage = deltaLage * lage; // operator* for quaternions
}
PRINTF("----------- Test END -------------------\n");
}
void GY_85::GyroCalibrate()
{
static int tmpx = 0;
static int tmpy = 0;
static int tmpz = 0;
g_offx = 0;
g_offy = 0;
g_offz = 0;
for( uint8_t i = 0; i < 10; i ++ ) //take the mean from 10 gyro probes and divide it from the current probe
{
delay(10);
float* gp = readGyro();
tmpx += *( gp);
tmpy += *(++gp);
tmpz += *(++gp);
}
g_offx = tmpx/10;
g_offy = tmpy/10;
g_offz = tmpz/10;
}
Window::Window(QWidget* parent)
: QWidget(parent)
{
QGridLayout* grid = new QGridLayout;
grid->addWidget(createAccelerometerGroup(), 0, 0);
grid->addWidget(createGyroscopeGroup(), 0, 1);
grid->addWidget(createMagnetometerGroup(), 1, 0);
grid->addWidget(createButtonsGroup(), 1, 1);
setLayout(grid);
setWindowTitle(tr("psmoveosc"));
resize(480, 320);
psm = new PSMoveQt(0);
psm->setColor(Qt::red);
psm->setEnabled(true);
connect(psm, SIGNAL(accelerometerChanged()),
this, SLOT(readAccelerometer()));
connect(psm, SIGNAL(gyroChanged()),
this, SLOT(readGyro()));
connect(psm, SIGNAL(magnetometerChanged()),
this, SLOT(readMagnetometer()));
connect(psm, SIGNAL(triggerChanged()),
this, SLOT(setTrigger()));
connect(psm, SIGNAL(buttonPressed(int)),
this, SLOT(onButton(int)));
connect(psm, SIGNAL(buttonReleased(int)),
this, SLOT(onButton(int)));
}
void ITG3200::readGyro(float *_GyroXYZ){
readGyro(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2);
}
void L3G4200D::readGyro(float *_GyroXYZ){
readGyro(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2);
}
int main(void)
{
int i;
Systick_Configuration();
LED_Configuration();
button_Configuration();
usart1_Configuration(9600);
SPI_Configuration();
TIM4_PWM_Init();
Encoder_Configration();
buzzer_Configuration();
ADC_Config();
//curSpeedX = 0;
//curSpeedW = 0;
//shortBeep(2000, 8000);
while(1) {
readSensor();
readGyro();
readVolMeter();
printf("LF %d RF %d DL %d DR %d aSpeed %d angle %d voltage %d lenc %d renc %d\r\n", LFSensor, RFSensor, DLSensor, DRSensor, aSpeed, angle, voltage, getLeftEncCount(), getRightEncCount());
displayMatrix("UCLA");
setLeftPwm(100);
setRightPwm(100);
delay_ms(1000);
}
//forwardDistance(4000,0,0,true);
//displayMatrix("Sped");
//targetSpeedX = 100;
//delay_ms(2000);
//
//targetSpeedX = 100;
//delay_ms(2000);
//printf("==============================================\n\r=======================================================\r\n");
//delay_ms(1000);
//displayMatrix("Wat");
//delay_ms(1000);
//displayMatrix("STOP");
//displayMatrix("GO");
turnRightAngle(LEFT);
targetSpeedW = 0;
delay_ms(1000);
turnRightAngle(RIGHT);
targetSpeedW = 0;
delay_ms(1000);
displayMatrix("STOP");
delay_ms(3000);
targetSpeedX = 0;
//
targetSpeedW = 10;
delay_ms(1000);
targetSpeedX = 0;
targetSpeedW = 0;
return 0;
}
void readSensors()
{
readGyro();
readAccel();
readMag();
}
void SysTick_Handler(void)
{
uint8_t index;
uint32_t currentTime;
sysTickCycleCounter = *DWT_CYCCNT;
sysTickUptime++;
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
if ((systemReady == true) &&
(accelCalibrating == false) &&
(gyroCalibrating == false) &&
(magCalibrating == false))
#endif
#if defined(USE_CHR6DM_AHRS)
if ((systemReady == true) &&
(accelCalibrating == false) &&
(gyroCalibrating == false) &&
(magCalibrating == false) &&
(ahrsCalibrating == false))
#endif
{
frameCounter++;
if (frameCounter > FRAME_COUNT)
frameCounter = 1;
///////////////////////////////
currentTime = micros();
deltaTime1000Hz = currentTime - previous1000HzTime;
previous1000HzTime = currentTime;
readAccel();
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
accelSum200Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum200Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum200Hz[ZAXIS] += rawAccel[ZAXIS].value;
#endif
accelSum100Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum100Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value;
readGyro();
gyroSum200Hz[ROLL] += rawGyro[ROLL].value;
gyroSum200Hz[PITCH] += rawGyro[PITCH].value;
gyroSum200Hz[YAW] += rawGyro[YAW].value;
gyroSum100Hz[ROLL] += rawGyro[ROLL].value;
gyroSum100Hz[PITCH] += rawGyro[PITCH].value;
gyroSum100Hz[YAW] += rawGyro[YAW].value;
gyroSum500Hz[ROLL] += rawGyro[ROLL].value;
gyroSum500Hz[PITCH] += rawGyro[PITCH].value;
gyroSum500Hz[YAW] += rawGyro[YAW].value;
///////////////////////////////
if ((frameCounter % COUNT_500HZ) == 0) {
frame_500Hz = true;
for (index = 0; index < 3; index++) {
gyroSummedSamples500Hz[index] = gyroSum500Hz[index];
gyroSum500Hz[index] = 0.0f;
}
}
///////////////////////////////
if ((frameCounter % COUNT_200HZ) == 0) {
frame_200Hz = true;
for (index = 0; index < 3; index++) {
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
accelSummedSamples200Hz[index] = accelSum200Hz[index];
accelSum200Hz[index] = 0.0f;
#endif
gyroSummedSamples200Hz[index] = gyroSum200Hz[index];
gyroSum200Hz[index] = 0.0f;
}
}
///////////////////////////////
if ((frameCounter % COUNT_100HZ) == 0) {
frame_100Hz = true;
for (index = 0; index < 3; index++) {
accelSummedSamples100Hz[index] = accelSum100Hz[index];
accelSum100Hz[index] = 0.0f;
gyroSummedSamples100Hz[index] = gyroSum100Hz[index];
gyroSum100Hz[index] = 0.0f;
}
if (frameCounter == COUNT_100HZ) {
readTemperatureRequestPressure();
} else if (frameCounter == FRAME_COUNT) {
readPressureRequestTemperature();
} else {
readPressureRequestPressure();
}
pressureSum += uncompensatedPressure.value;
}
///////////////////////////////
if (((frameCounter + 1) % COUNT_50HZ) == 0) {
readMag();
magSum[XAXIS] += rawMag[XAXIS].value;
magSum[YAXIS] += rawMag[YAXIS].value;
magSum[ZAXIS] += rawMag[ZAXIS].value;
}
if ((frameCounter % COUNT_50HZ) == 0) {
frame_50Hz = true;
}
///////////////////////////////
if ((frameCounter % COUNT_10HZ) == 0)
frame_10Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_5HZ) == 0)
frame_5Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_1HZ) == 0)
frame_1Hz = true;
///////////////////////////////////
executionTime1000Hz = micros() - currentTime;
///////////////////////////////
}
}
task MapRoom()
{
float botLastXCoordinate = 0.0;
float botLastYCoordinate = 0.0;
float botCurrentXCoordinate = 0.0;
float botCurrentYCoordinate = 0.0;
int wallXCoordinate = 0;
int wallYCoordinate = 0;
int xCoordinateDisplayOffset = 50;
int yCoordinateDisplayOffset = 40;
startGyro();
resetGyro();
eraseDisplay();
while(true)
{
//Robot position
botCurrentXCoordinate = botLastXCoordinate + EncoderDistance(ReadEncoderValue()) * sinDegrees(readGyro());
botCurrentYCoordinate = botLastYCoordinate + EncoderDistance(ReadEncoderValue()) * cosDegrees(readGyro());
//Wall mapping
wallXCoordinate = botCurrentXCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * cosDegrees(readGyro());
wallYCoordinate = botCurrentYCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * sinDegrees(readGyro());
nxtSetPixel(wallXCoordinate + xCoordinateDisplayOffset, wallYCoordinate + yCoordinateDisplayOffset);
ResetEncoderValue();
botLastXCoordinate = botCurrentXCoordinate;
botLastYCoordinate = botCurrentYCoordinate;
wait1Msec(20);
}
}
int main()
{
int32_t bemf_vals[4] = {0,0,0,0};
int32_t bemf_vals_filt[4] = {0,0,0,0};
init();
// set up DMA/SPI buffers
init_rx_buffer();
init_tx_buffer();
// set up pid structs
pid_struct pid_structs[4];
{ uint8_t i;
for (i = 0; i < 4; ++i) init_pid_struct(&(pid_structs[i]), i);
}
update_dig_pin_configs();
config_adc_in_from_regs();
//debug_printf("starting\n");
int low_volt_alarmed = 0;
// Loop until button is pressed
uint32_t count = 0;
while (1)
{
count += 1;
{
// only sample motor backemf 1/4 of the time
const uint8_t bemf_update_time = (count % 4 == 1);
if (bemf_update_time)
{
// idle breaking
MOT0_DIR1_PORT->BSRRH |= MOT0_DIR1;
MOT0_DIR2_PORT->BSRRH |= MOT0_DIR2;
MOT1_DIR1_PORT->BSRRH |= MOT1_DIR1;
MOT1_DIR2_PORT->BSRRH |= MOT1_DIR2;
MOT2_DIR1_PORT->BSRRH |= MOT2_DIR1;
MOT2_DIR2_PORT->BSRRH |= MOT2_DIR2;
MOT3_DIR1_PORT->BSRRH |= MOT3_DIR1;
MOT3_DIR2_PORT->BSRRH |= MOT3_DIR2;
}
// let the motor coast
// delay_us(700);
// now I squeeze in most sensor updates instead of just sleeping
uint32_t before = usCount;
update_dig_pins();
int16_t batt = adc_update();
readAccel();
readMag();
readGyro();
if (batt < 636) // about 5.75 volts
{
configDigitalOutPin(LED1_PIN, LED1_PORT);
low_volt_alarmed = 1;
if (count % 50 < 10) // low duty cycle to save battery
{
LED1_PORT->BSRRL |= LED1_PIN; // ON
}
else
{
LED1_PORT->BSRRH |= LED1_PIN; // OFF
}
}
else if (low_volt_alarmed)
{
// make sure led is off coming out of the low voltage alarm
configDigitalOutPin(LED1_PIN, LED1_PORT);
LED1_PORT->BSRRH |= LED1_PIN; // OFF
low_volt_alarmed = 0;
}
if (adc_dirty)
{
adc_dirty = 0;
config_adc_in_from_regs();
}
if (dig_dirty)
{
dig_dirty = 0;
update_dig_pin_configs();
}
uint32_t sensor_update_time = usCount - before;
const uint16_t us_delay_needed = 700;
const uint8_t got_time_to_burn = sensor_update_time < us_delay_needed;
if (got_time_to_burn)
{
// sleep the remainder of the coasting period before sampling bemf
delay_us(us_delay_needed-sensor_update_time);
}
if (bemf_update_time)
{
update_bemfs(bemf_vals, bemf_vals_filt);
// set the motor directions back up
update_motor_modes();
uint8_t channel;
for (channel = 0; channel < 4; ++channel)
{
uint8_t shift = 2*channel;
uint8_t motor_mode = (aTxBuffer[REG_RW_MOT_MODES] & (0b11 << shift)) >> shift;
motor_update(bemf_vals[channel], bemf_vals_filt[channel], &pid_structs[channel], channel, motor_mode);
}
}
else
{
// updating sensors doesnt' take as long as all of the adc for backemf updates
// sleep a bit so that our time through the loop is consistent for PID control
delay_us(222);
}
}
}
void SysTick_Handler(void)
{
uint8_t index;
uint32_t currentTime;
sysTickCycleCounter = *DWT_CYCCNT;
sysTickUptime++;
if ((systemReady == true ) &&
(cliBusy == false) &&
(accelCalibrating == false) &&
(escCalibrating == false) &&
(gyroCalibrating == false) &&
(magCalibrating == false))
{
frameCounter++;
if (frameCounter > FRAME_COUNT)
frameCounter = 1;
///////////////////////////////
currentTime = micros();
deltaTime1000Hz = currentTime - previous1000HzTime;
previous1000HzTime = currentTime;
readAccel();
accelSum200Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum200Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum200Hz[ZAXIS] += rawAccel[ZAXIS].value;
accelSum100Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum100Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value;
readGyro();
gyroSum200Hz[ROLL ] += rawGyro[ROLL ].value;
gyroSum200Hz[PITCH] += rawGyro[PITCH].value;
gyroSum200Hz[YAW ] += rawGyro[YAW ].value;
///////////////////////////////
if ((frameCounter % COUNT_200HZ) == 0)
{
frame_200Hz = true;
for (index = 0; index < 3; index++)
{
accelSummedSamples200Hz[index] = accelSum200Hz[index];
accelSum200Hz[index] = 0.0f;
gyroSummedSamples200Hz[index] = gyroSum200Hz[index];
gyroSum200Hz[index] = 0.0f;
}
}
///////////////////////////////
if ((frameCounter % COUNT_100HZ) == 0)
{
frame_100Hz = true;
for (index = 0; index < 3; index++)
{
accelSummedSamples100Hz[index] = accelSum100Hz[index];
accelSum100Hz[index] = 0.0f;
}
if (frameCounter == COUNT_100HZ)
readTemperatureRequestPressure();
else if (frameCounter == FRAME_COUNT)
readPressureRequestTemperature();
else
readPressureRequestPressure();
pressureSum += uncompensatedPressure.value;
}
///////////////////////////////
if ((frameCounter % COUNT_50HZ) == 0)
{
frame_50Hz = true;
}
///////////////////////////////
if (((frameCounter + 1) % COUNT_10HZ) == 0)
newMagData = readMag();
if ((frameCounter % COUNT_10HZ) == 0)
frame_10Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_5HZ) == 0)
frame_5Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_1HZ) == 0)
frame_1Hz = true;
///////////////////////////////////
executionTime1000Hz = micros() - currentTime;
///////////////////////////////
}
}
void SAT_Gyro::readGyro(float *_GyroXYZ){
readGyro(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2);
}
int ITG3200::readGyro(float *_GyroXYZ){
int a=0;
a=readGyro(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2);
return a;
}
void mpu9150::getGyro(double *returnArray){
readGyro();
returnArray[0] = this->xGyro;
returnArray[1] = this->yGyro;
returnArray[2] = this->zGyro;
}
