task gyroTask() {
initGyro();
while(true) {
updateHeading();
angle2 = angle;
}
}
uint16_t LSM9DS0_begin( LSM9DS0_t* lsm_t, LMS9DS0_Init_t* init_t )
{
#if (DEBUG0==1)
uint8_t test[8];
#endif
uint8_t gTest, xmTest;
if(init_t==NULL)
init_t = &LMS_Device_Defaults;
// Store the given scales in class variables. These scale variables
// are used throughout to calculate the actual g's, DPS,and Gs's.
lsm_t->gScale = init_t->gScl;
lsm_t->aScale = init_t->aScl;
lsm_t->mScale = init_t->mScl;
// Once we have the scale values, we can calculate the resolution
// of each sensor. That's what these functions are for. One for each sensor
calcgRes(lsm_t); // Calculate DPS / ADC tick, stored in gRes variable
calcmRes(lsm_t); // Calculate Gs / ADC tick, stored in mRes variable
calcaRes(lsm_t); // Calculate g / ADC tick, stored in aRes variable
// Now, initialize our hardware interface.
#if(LSM_I2C_SUPPORT==1)
if (lsm_t->interfaceMode == MODE_I2C) // If we're using I2C
LSM_initI2C(); // Initialize I2C
else if (lsm_t->interfaceMode == MODE_SPI) // else, if we're using SPI
#endif
//LSM_initSPI(); // Initialize SPI
// To verify communication, we can read from the WHO_AM_I register of
// each device. Store those in a variable so we can return them.
gTest = gReadByte(lsm_t,WHO_AM_I_G); // Read the gyro WHO_AM_I
xmTest = xmReadByte(lsm_t,WHO_AM_I_XM); // Read the accel/mag WHO_AM_I
// Gyro initialization stuff:
initGyro(lsm_t); // This will "turn on" the gyro. Setting up interrupts, etc.
setGyroODR(lsm_t,init_t->gODR); // Set the gyro output data rate and bandwidth.
setGyroScale(lsm_t,lsm_t->gScale); // Set the gyro range
// Accelerometer initialization stuff:
initAccel(lsm_t); // "Turn on" all axes of the accel. Set up interrupts, etc.
setAccelODR(lsm_t,init_t->aODR); // Set the accel data rate.
setAccelScale(lsm_t,lsm_t->aScale); // Set the accel range.
// Magnetometer initialization stuff:
initMag(lsm_t); // "Turn on" all axes of the mag. Set up interrupts, etc.
setMagODR(lsm_t,init_t->mODR); // Set the magnetometer output data rate.
setMagScale(lsm_t,lsm_t->mScale); // Set the magnetometer's range.
LSM9DS0_Update_Ctrl(lsm_t);
lsm_t->update=UPDATE_ON_SET;
#if (DEBUG0==1)
I2CreadBytes(lsm_t->gAddress,CTRL_REG1_G,test,5);
I2CreadBytes(lsm_t->xmAddress,CTRL_REG0_XM,test,7);
#endif
// Once everything is initialized, return the WHO_AM_I registers we read:
return (xmTest << 8) | gTest;
}
uint16_t begin(LSM9DS0_t* imu, gyro_scale gScl, accel_scale aScl,
mag_scale mScl, gyro_odr gODR, accel_odr aODR, mag_odr mODR)
{
// Store the given scales in imu struct. These scale variables
// are used throughout to calculate the actual g's, DPS,and Gs's.
imu->gScale = gScl;
imu->aScale = aScl;
imu->mScale = mScl;
// Once we have the scale values, we can calculate the resolution
// of each sensor. That's what these functions are for. One for each sensor
calcgRes(imu); // Calculate DPS / ADC tick, stored in gRes variable
calcaRes(imu); // Calculate g / ADC tick, stored in aRes variable
calcmRes(imu); // Calculate Gs / ADC tick, stored in mRes variable
// To verify communication, we can read from the WHO_AM_I register of
// each device. Store those in a variable so we can return them.
uint8_t gTest = gReadByte(imu->gyro, WHO_AM_I_G); // Read the gyro WHO_AM_I
uint8_t xmTest = xmReadByte(imu->xm, WHO_AM_I_XM); // Read the accel/mag WHO_AM_I
// Gyro initialization stuff:
initGyro(imu->gyro); // This will "turn on" the gyro. Setting up interrupts, etc.
setGyroODR(imu->gyro, gODR); // Set the gyro output data rate and bandwidth.
setGyroScale(imu, imu->gScale); // Set the gyro range
}
/* ************************************************************************** */
uint16_t LSM9DS0_begin_adv( stLSM9DS0_t * stThis,
gyro_scale gScl,
accel_scale aScl,
mag_scale mScl,
gyro_odr gODR,
accel_odr aODR,
mag_odr mODR )
{
// Default to 0xFF to indicate status is not OK
uint8_t gTest = 0xFF;
uint8_t xmTest = 0xFF;
byte byDatas;
byte byAddress = 0x1D;
byte bySubAddress = 0x0F;
// Wiat for a few millis at the beginning for the chip to boot
WAIT1_Waitms( 200 );
// Store the given scales in class variables. These scale variables
// are used throughout to calculate the actual g's, DPS,and Gs's.
stThis->gScale = gScl;
stThis->aScale = aScl;
stThis->mScale = mScl;
// Once we have the scale values, we can calculate the resolution
// of each sensor. That's what these functions are for. One for each sensor
calcgRes(stThis); // Calculate DPS / ADC tick, stored in gRes variable
calcmRes(stThis); // Calculate Gs / ADC tick, stored in mRes variable
calcaRes(stThis); // Calculate g / ADC tick, stored in aRes variable
// Now, initialize our hardware interface.
if (stThis->interfaceMode == MODE_I2C) // If we're using I2C
initI2C(stThis); // Initialize I2C
else if (stThis->interfaceMode == MODE_SPI) // else, if we're using SPI
initSPI(stThis); // Initialize SPI
// To verify communication, we can read from the WHO_AM_I register of
// each device. Store those in a variable so we can return them.
gTest = gReadByte( stThis, WHO_AM_I_G ); // Read the gyro WHO_AM_I
xmTest = xmReadByte( stThis, WHO_AM_I_XM ); // Read the accel/mag WHO_AM_I
// Gyro initialization stuff:
initGyro(stThis); // This will "turn on" the gyro. Setting up interrupts, etc.
LSM9DS0_setGyroODR(stThis, gODR); // Set the gyro output data rate and bandwidth.
LSM9DS0_setGyroScale(stThis, stThis->gScale); // Set the gyro range
// Accelerometer initialization stuff:
initAccel(stThis); // "Turn on" all axes of the accel. Set up interrupts, etc.
LSM9DS0_setAccelODR(stThis, aODR); // Set the accel data rate.
LSM9DS0_setAccelScale(stThis, stThis->aScale); // Set the accel range.
// Magnetometer initialization stuff:
initMag(stThis); // "Turn on" all axes of the mag. Set up interrupts, etc.
LSM9DS0_setMagODR(stThis, mODR); // Set the magnetometer output data rate.
LSM9DS0_setMagScale(stThis, stThis->mScale); // Set the magnetometer's range.
// Once everything is initialized, return the WHO_AM_I registers we read:
return (xmTest << 8) | gTest;
}
uint16_t LSM9DS1::begin()
{
//! Todo: don't use ACCEL_GYRO_ADDRESS or MAGNETOMETER_ADDRESS, duplicating memory
constrainScales();
// Once we have the scale values, we can calculate the resolution
// of each sensor. That's what these functions are for. One for each sensor
calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
calcaRes(); // Calculate g / ADC tick, stored in aRes variable
initI2C(); // Initialize I2C
// To verify communication, we can read from the WHO_AM_I register of
// each device. Store those in a variable so we can return them.
uint8_t mTest = mReadByte(WHO_AM_I_M); // Read the gyro WHO_AM_I
uint8_t xgTest = xgReadByte(WHO_AM_I_XG); // Read the accel/mag WHO_AM_I
uint16_t whoAmICombined = (xgTest << 8) | mTest;
if (whoAmICombined != ((WHO_AM_I_AG_RSP << 8) | WHO_AM_I_M_RSP))
return 0;
// Gyro initialization stuff:
initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
// Accelerometer initialization stuff:
initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
// Magnetometer initialization stuff:
initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
// Once everything is initialized, return the WHO_AM_I registers we read:
return whoAmICombined;
}
int main(void) {
thread_t *sh = NULL;
PollerData.temp = 0;
PollerData.press = 0;/*
PollerData.uTime = 0;*/
halInit();
chSysInit();
shellInit();
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1000);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
// SPI-related pins (for display)
palSetPadMode(GPIOB, 11, PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode(GPIOB, 10, PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode(GPIOB, 13, PAL_MODE_ALTERNATE(5));
palSetPadMode(GPIOB, 14, PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode(GPIOB, 15, PAL_MODE_ALTERNATE(5));
spiStart(&SPID1, &spi1cfg);
spiStart(&SPID2, &spi2cfg);
i2cStart(&I2CD1, &i2cconfig);
initGyro();
initAccel();
initMag();
// nunchuk_status = nunchuk_init();
bmp085_status = bmp085_init();
lcd5110Init();
lcd5110SetPosXY(0, 0);
lcd5110WriteText("P :: ");
lcd5110SetPosXY(0, 1);
lcd5110WriteText("T :: ");
chThdCreateStatic(waThreadBlink, sizeof(waThreadBlink), NORMALPRIO, ThreadBlink, NULL);
chThdCreateStatic(waThreadButton, sizeof(waThreadButton), NORMALPRIO, ThreadButton, NULL);
chThdCreateStatic(waPoller, sizeof(waPoller), NORMALPRIO, ThreadPoller, NULL);
while (TRUE) {
if (!sh) {
sh = shellCreate(&shCfg, SHELL_WA_SIZE, NORMALPRIO);
}
else if (chThdTerminatedX(sh)) {
chThdRelease(sh);
sh = NULL;
}
chThdSleepMilliseconds(1000);
}
return 0; // never returns, lol
}
uint8_t Init_IMU(void){
init_i2c();
__delay_cycles(800);
if (initGyro()){return 1;}
__delay_cycles(800);
if (initXM()){return 1;}
__delay_cycles(800);
return 0;
}
int main(void) {
WDTCTL = WDTPW + WDTHOLD; // Stop WDT
PM5CTL0 &= ~LOCKLPM5;
//Set clock speed
CSCTL0_H = CSKEY >> 8; // Unlock CS registers
CSCTL1 = DCOFSEL_6; // Set DCO to 8MHz
CSCTL2 = SELA__VLOCLK | SELS__DCOCLK | SELM__DCOCLK; // Set SMCLK = MCLK = DCO
// ACLK = VLOCLK
CSCTL3 = DIVA__1 | DIVS__1 | DIVM__1; // Set all dividers to 1
CSCTL0_H = 0; // Lock CS registers
//Set clock speed
rate_gyr_x = 0;
rate_gyr_y = 0;
rate_gyr_z = 0;
init_i2c();
initGyro();
initXM();
while (1) {
////////////READ GYROSCOPE//////////////
xRate_L = single_byte_read_i2c(gyro_Address, OUT_X_L_G);
xRate_H = single_byte_read_i2c(gyro_Address, OUT_X_H_G);
yRate_L = single_byte_read_i2c(gyro_Address, OUT_Y_L_G);
yRate_H = single_byte_read_i2c(gyro_Address, OUT_Y_H_G);
zRate_L = single_byte_read_i2c(gyro_Address, OUT_Z_L_G);
zRate_H = single_byte_read_i2c(gyro_Address, OUT_Z_H_G);
/////////////READ ACCELEROMETER/////////////////
xaccel_L = single_byte_read_i2c(XMAddress, OUT_X_L_A);
xaccel_H = single_byte_read_i2c(XMAddress, OUT_X_H_A);
yaccel_L = single_byte_read_i2c(XMAddress, OUT_Y_L_A);
yaccel_H = single_byte_read_i2c(XMAddress, OUT_Y_H_A);
zaccel_L = single_byte_read_i2c(XMAddress, OUT_Z_L_A);
zaccel_H = single_byte_read_i2c(XMAddress, OUT_Z_H_A);
////////////////CONVERT TO MEANINGFUL DATA//////////////////////
gyrRawx = (xRate_L | xRate_H << 8);
gyrRawy = (yRate_L | yRate_H << 8);
gyrRawz = (zRate_L | zRate_H << 8);
rate_gyr_x = gyrRawx * G_GAIN;
rate_gyr_y = gyrRawy * G_GAIN;
rate_gyr_z = gyrRawz * G_GAIN;
accRawx = (xaccel_L | xaccel_H << 8);
accRawy = (yaccel_L | yaccel_H << 8);
accRawz = (zaccel_L | zaccel_H << 8);
g_x = (accRawx * A_GAIN) / thousand;
g_y = (accRawy * A_GAIN) / thousand;
g_z = (accRawz * A_GAIN) / thousand;
}
}
void initSensors()
{
//_delay_ms(300);
for (a=0; a<3; a++) {_delay_loop_2(65535);} // ca. 30 ms
initMPU6050();
initGyro();
initAcc();
//_delay_ms(300);
for (a=0; a<3; a++) {_delay_loop_2(65535);} // ca. 30 ms
}
task main()
{
initGyro();
waitForStart();
//wait1Msec(5000);
driveTo(10000);
turnDegrees(90);
resetEncoders();
driveTo(-4000, 90);
turnDegrees(60);
resetEncoders();
driveTo(-17000, 60);
resetEncoders();
driveTo(500);
holdSpinners();
}
task main()
{
initGyro();
// waitForStart();
// wait1Msec(2000);
driveTo(10000); // come off the ramp
turnDegrees(-90); // turn so we are pointed to the ball
resetEncoders();
driveTo(-6500, -90); // drive towards ball
turnDegrees(-50); // angle toward ball so we can push it and end up inside the park zone
resetEncoders();
driveTo(-15000, -43); // drive into zone
resetEncoders();
driveTo(1000);
holdSpinners();
}
uint16_t begin(void)
{
//! Todo: don't use _xgAddress or _mAddress, duplicating memory
_xgAddress = settings.device.agAddress;
_mAddress = settings.device.mAddress;
constrainScales();
// Once we have the scale values, we can calculate the resolution
// of each sensor. That's what these functions are for. One for each sensor
calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
calcaRes(); // Calculate g / ADC tick, stored in aRes variable
if (settings.device.commInterface == IMU_MODE_I2C) // If we're using I2C
initI2C(); // Initialize I2C
else if (settings.device.commInterface == IMU_MODE_SPI) // else, if we're using SPI
initSPI(); // Initialize SPI
// To verify communication, we can read from the WHO_AM_I register of
// each device. Store those in a variable so we can return them.
uint8_t mTest = mReadByte(WHO_AM_I_M); // Read the gyro WHO_AM_I
delay(DELAY * 150);
uint8_t xgTest = xgReadByte(WHO_AM_I_XG); // Read the accel/mag WHO_AM_I
uint16_t whoAmICombined = (xgTest << 8) | mTest;
if (whoAmICombined != ((WHO_AM_I_AG_RSP << 8) | WHO_AM_I_M_RSP))
{
return 0;
}
// Gyro initialization stuff:
initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
// Accelerometer initialization stuff:
initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
// Magnetometer initialization stuff:
initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
// Once everything is initialized, return the WHO_AM_I registers we read:
return whoAmICombined;
}
uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl,
gyro_odr gODR, accel_odr aODR, mag_odr mODR)
{
// Store the given scales in class variables. These scale variables
// are used throughout to calculate the actual g's, DPS,and Gs's.
gScale = gScl;
aScale = aScl;
mScale = mScl;
// Once we have the scale values, we can calculate the resolution
// of each sensor. That's what these functions are for. One for each sensor
calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
calcaRes(); // Calculate g / ADC tick, stored in aRes variable
// Now, initialize our hardware interface.
if (interfaceMode == MODE_I2C) // If we're using I2C
{} //initI2C(); // Initialize I2C
else if (interfaceMode == MODE_SPI) // else, if we're using SPI
initSPI(); // Initialize SPI
// To verify communication, we can read from the WHO_AM_I register of
// each device. Store those in a variable so we can return them.
uint8_t gTest = gReadByte(WHO_AM_I_G); // Read the gyro WHO_AM_I
uint8_t xmTest = xmReadByte(WHO_AM_I_XM); // Read the accel/mag WHO_AM_I
// Gyro initialization stuff:
initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
setGyroScale(gScale); // Set the gyro range
// Accelerometer initialization stuff:
initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
setAccelODR(aODR); // Set the accel data rate.
setAccelScale(aScale); // Set the accel range.
// Magnetometer initialization stuff:
initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
setMagODR(mODR); // Set the magnetometer output data rate.
setMagScale(mScale); // Set the magnetometer's range.
// Once everything is initialized, return the WHO_AM_I registers we read:
return (xmTest << 8) | gTest;
}
task main()
{
writeValueToLEDs(0);
autonomousOptimized = true;
initSwerve();
initLift();
lineFollowerInit();
writeValueToLEDs(IDLE);
waitForStart();
initGyro();
setModulePositions(1440);
resetDistance();
while ( !driveToCenter(false) )
{
updateGyro();
updateSwerve();
updateLift();
}
}
/** Power on and prepare for general usage.
* This will activate the device and take it out of sleep mode (which must be done
* after start-up). This function also sets both the accelerometer and the gyroscope
* to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
* the clock source to use the X Gyro for reference, which is slightly better than
* the default internal clock source.
*/
PUBLIC void initializeLSM6DS0(bool useAccel, bool useGyro) {
xAccelOffset = yAccelOffset = zAccelOffset = 0;
xGyroOffset = yGyroOffset = zGyroOffset = 0;
gyroEnabled = useGyro;
accelEnabled = useAccel;
if(gyroEnabled)
{
gScale = GYRO_FS_245DSP;
calcGyroRes();
}
if(accelEnabled)
{
aScale = ACC_FS_2G;
calcAccelRes();
}
if(testConnection())
{
resetLSM6DS0();
if(accelEnabled)
{
initAccel();
setFullScaleAccelRange(aScale);
setAccelODR(ACC_ODR_119Hz);
}
if(gyroEnabled)
{
initGyro();
setFullScaleGyroRange(gScale);
setGyroODR(GYRO_ODR_119Hz_CO_14Hz);
}
calibrateLSM6DS0(gbias,abias);
}
}
void systemInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
// Turn on clocks for stuff we use
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE);
RCC_ClearFlag();
// Make all GPIO in by default to save power and reduce noise
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_Init(GPIOC, &GPIO_InitStructure);
// Turn off JTAG port 'cause we're using the GPIO for leds
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
checkFirstTime(false);
readEEPROM();
ledInit();
LED0_ON;
initMixer();
pwmOutputConfig.escPwmRate = eepromConfig.escPwmRate;
pwmOutputConfig.servoPwmRate = eepromConfig.servoPwmRate;
cliInit(115200);
i2cInit(I2C2);
pwmOutputInit(&pwmOutputConfig);
rxInit();
delay(20000); // 20 sec delay for sensor stabilization - probably not long enough.....
LED1_ON;
initAccel();
initGyro();
initMag();
initPressure();
initPID();
}
/* =================================================
FUNCTION: initIMU
CREATED: 16-05-2014
DESCRIPTION: This function initializes the IMU
sensor pack
PARAMETERS: None
GLOBAL VARIABLES: None.
RETURNS: None.
AUTHOR: P. Kantue
================================================== */
void initIMU()
{
initAcc();
initGyro();
initMagn();
}
int main(void) {
volatile uint32_t valore = 0, i, blink = 0, contatore, lampeggio_led;
volatile int32_t arrot;
volatile int16_t val1 = 0, x, y, z;
distanza DIST;
//--------------------------//
///definizione strutture/////
//-------------------------//
//volatile double d = 1.9845637456;
gyro G;
accelerazione A;
cinematica CIN;
/// servono differenti PID, almeno uno per la rotazione ed uno per lo spostamento
/// per la rotazione sarebbero interessante usarne 2, uno per la ortazione soft ed uno per la rotazione
/// brusca.
pid CTRL[3], * pidPtr;
/// descrittore della sintassi dei comandi
syn_stat synSTATO;
/// modulo zigbee per telemetria
xbee XB;
/// pwm servi e motori
pwm PWM, pwmServi;
/// struttura del sensore di colore
colore COL;
/// sensore di temperatura ad infrarossi
temperatura TEMP;
TEMPER sensIR;
/// indormazioni sul sopravvissuto
survivor SUR;
//inizializzazione struttura per qei
qei QEI;
/// oggetto che riallinea il mezzo
allineamento AL;
/// disabilita le interruzioni
DI();
pidPtr = CTRL;
dPtr = &DIST;
TEMPptr = &TEMP;
CIN.Aptr = &A;
CIN.distPTR = &DIST;
CIN.vel = 0.0;
dati DATA;
//passaggio degli indirizzi delle strutture alla struttura generale
dati_a_struttura(&G, &DIST, &CIN, &COL, &TEMP, &SUR, &DATA);
/// commento per provare il merge su server remoto
/// setup di base
setupMCU();
/// imposta i parametri del PID
setupPID(CTRL);
/// imposta le UART
setupUART();
//inizializzo l'i2c
InitI2C0();
/// messaggio d'inizio
PRINT_WELCOME();
/// inizializza il giroscopio
initGyro(&G, Z_AXIS);
/// inizializza il timer 0 e genera un tick da 10 ms
initTimer0(10, &G);
/// inizializza il timer 1
initTimer1(100);
/// inizializza il contatore della persistenza del comando
synSTATO.tick = 0;
/// inizializza il pwm
pwmMotInit(&PWM);
// TODO: //pwmServoInit (&pwmServi);
/// inizializza l'adc e lo prepara a funzionare ad interruzioni.
initAdc(&DIST);
/// reset dell'automa di analisi della sintassi
resetAutoma(&synSTATO);
//servo = (pwm *) &pwmServi;
/// iniziailizzazione del lettore encoder
qei_init(&QEI);
/// abilita le interruzioni
EI();
/// attende che il sensore vada a regime
if (G.IsPresent == OK){
PRINTF("\nAzzeramento assi giroscopio\n");
while (blink < 70){
if (procCom == 1){
procCom = 0;
blink++;
}
}
blink = 0;
/// azzeramento degli assi
azzeraAssi(&G);
}
/// test della presenza del modulo zig-bee
/// il modulo zig-be si attiva con al sequnza '+++' e risponde con 'OK' (maiuscolo)
if (testXbee() == 0){
// ok;
XB.present = 1;
PRINTF("Modulo xbee presente.\n");
}
else{
XB.present = 0;
PRINTF("Modulo xbee non presente.\n");
}
pwm_power(&PWM);
contatore = 0;
//// inizializza l'accelrometro
//stato = writeI2CByte(CTRL_REG1_A, ODR1 + ODR0 + ZaxEN + YaxEN + XaxEN);
// scrivo nel registro 0x20 il valore 0x0F, cioe' banda minima, modulo on e assi on
/// sintassi: indirizzo slave, num parm, indirizzo reg, valore da scrivere
//I2CSend(ACCEL_ADDR, 2, CTRL_REG1_A, ODR1 + ODR0 + ZaxEN + YaxEN + XaxEN);
A.isPresent = testAccel();
if (A.isPresent)
impostaAccel(&A);
/// taratura sul sensore di luminosita'
whiteBal(&COL);
/// taratura del sensore di temepratura
taraturaTemp(&TEMP);
///
qei_test(&QEI);
/// task principale
while(1){
/// invia la risposta per i comandi di rotazione, quando sono stati eseguiti
if(pidPtr->rispondi == TRUE){
rispostaRotazione(pidPtr, &synSTATO);
pidPtr->rispondi = FALSE;
}
if (procCom == 1 ){
//UARTCharPutNonBlocking(UART1_BASE, 'c');
procCom = 0;
contatore++;
lampeggio_led++;
if(lampeggio_led >= 50)
{
lampeggio_led = 0;
if(DATA.surPtr->isSurvivor == TRUE )
{
if(HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + (GREEN_LED << 2))) != GREEN_LED )
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + (RED_LED << 2))) = 0;
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + (GREEN_LED | RED_LED << 2))) ^= GREEN_LED | RED_LED;
}
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + (GREEN_LED << 2))) ^= GREEN_LED;
}
/* LETTURA DEL COMANDO */
/// restituisce l'indirizzo del PID da utilizzare nel successivo processo di calcolo
pidPtr = leggiComando(&synSTATO, CTRL, pidPtr, &DATA);
/* LETTURA SENSORI */
/// effettua i calcoli solo se il giroscopio e' presente
/// TODO: il PID viene calcolato ongi 10ms oppure ogni 20ms? Come è meglio?
/* misura gli encoder e calcola spostameti e velocità */
/* misura i sensori di distanza */
if (DIST.run == true)
/// TODO controllare se riesce a funzionare mentre legge le accelerazioni su I2C
ROM_ADCProcessorTrigger(ADC0_BASE, 0);
/// misura i dati forniti dall'accelerometro se disponibili
if(A.isPresent)
misuraAccelerazioni(&A);
/// le misure del giroscopio invece sono effettuate solo dall'apposito pid
/*if(G.IsPresent == OK)
if( contatore == 1){
/// ogni 10 ms effettua il calcolo del PID
contatore = 0;
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + (GPIO_PIN_0 << 2))) |= GPIO_PIN_0;
PID(&G, pidPtr, &PWM, &CIN);
setXPWM(&CTRL[1], &PWM);
procCom = 0;
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + (GPIO_PIN_0 << 2))) &= ~GPIO_PIN_0;
blink++;
/// lampeggio del led con periodo di 2 s.
if (blink >= 100){
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + ((GPIO_PIN_2 | GPIO_PIN_1) << 2))) = 0;
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + (GPIO_PIN_3 << 2))) ^= GPIO_PIN_3;
blink = 0;
}
///provvede ad integrare la misura della velcita' angolare ogni 10 ms
//misuraAngoli(&G);
//PRINTF("asse x: %d\t", G.pitch);
//PRINTF("\tasse y: %d\t", G.roll);
//PRINTF("\tasse z: %d\n", G.yaw);
//PRINTF("uscita PID: %d\n", C.uscita);
}*/
/* RISPOSTA AL COMANDO */
inviaSensore(&synSTATO, &DATA);
}
}
}
int main(void) {
int16 acc[3];
int16 gyro[3];
int16 mag[3];
int16 temperature = 0;
int32 pressure = 0;
int32 centimeters = 0;
i2c_master_enable(I2C1, I2C_FAST_MODE);
delay(200);
initAcc();
delay(200);
initGyro();
delay(200);
zeroCalibrateGyroscope(128,5);
compassInit(false);
compassCalibrate(1);
compassSetMode(0);
bmp085Calibration();
while(1) {
getAccelerometerData(acc); //Read acceleration
SerialUSB.print("Accel: ");
SerialUSB.print(acc[0]);
SerialUSB.print(" ");
SerialUSB.print(acc[1]);
SerialUSB.print(" ");
SerialUSB.print(acc[2]);
getGyroscopeData(gyro); //Read acceleration
SerialUSB.print(" Gyro: ");
SerialUSB.print(gyro[0]);
SerialUSB.print(" ");
SerialUSB.print(gyro[1]);
SerialUSB.print(" ");
SerialUSB.print(gyro[2]);
getMagnetometerData(mag); //Read acceleration
SerialUSB.print(" Mag: ");
SerialUSB.print(mag[0]);
SerialUSB.print(" ");
SerialUSB.print(mag[1]);
SerialUSB.print(" ");
SerialUSB.print(mag[2]);
temperature = bmp085GetTemperature(bmp085ReadUT());
pressure = bmp085GetPressure(bmp085ReadUP());
centimeters = bmp085GetAltitude();
SerialUSB.print(" Temp: ");
SerialUSB.print(temperature, DEC);
SerialUSB.print(" *0.1 deg C ");
SerialUSB.print("Pressure: ");
SerialUSB.print(pressure, DEC);
SerialUSB.print(" Pa ");
SerialUSB.print("Altitude: ");
SerialUSB.print(centimeters, DEC);
SerialUSB.print(" cm ");
SerialUSB.println(" ");
delay(100);
}
return 0;
}
void testInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
uint8_t i;
struct __gpio_config_t {
GPIO_TypeDef *gpio;
uint16_t pin;
GPIOMode_TypeDef mode;
}
gpio_cfg[] = {
{LED0_GPIO, LED0_PIN, GPIO_Mode_Out_PP}, // PB3 (LED)
{LED1_GPIO, LED1_PIN, GPIO_Mode_Out_PP}, // PB4 (LED)
};
uint8_t gpio_count = sizeof(gpio_cfg) / sizeof(gpio_cfg[0]);
// Turn on clocks for stuff we use
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE);
RCC_ClearFlag();
// Make all GPIO in by default to save power and reduce noise
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_Init(GPIOC, &GPIO_InitStructure);
// Turn off JTAG port 'cause we're using the GPIO for leds
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
// Configure gpio
for (i = 0; i < gpio_count; i++) {
GPIO_InitStructure.GPIO_Pin = gpio_cfg[i].pin;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = gpio_cfg[i].mode;
GPIO_Init(gpio_cfg[i].gpio, &GPIO_InitStructure);
}
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
LED0_OFF;
LED1_OFF;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
checkFirstTime(true,true);
initMixer();
pwmOutputConfig.motorPwmRate = 10*1000;
pwmOutputConfig.noEsc = true;
pwmOutputInit(&pwmOutputConfig);
i2cInit(SENSOR_I2C);
initGyro();
initAccel();
initMag();
nrf_init();
nrf_detect();
}
//initializes Gyro
void initializeRobot()
{
initGyro(S2);
//servo[stand] = 90;
//servo[ball] = 18;
}
