bool sensorsAutodetect(void)
{
// gyro must be initialised before accelerometer
bool gyroDetected = gyroInit();
#ifdef USE_ACC
if (gyroDetected) {
accInit(gyro.targetLooptime);
}
#endif
#ifdef USE_MAG
compassInit();
#endif
#ifdef USE_BARO
baroDetect(&baro.dev, barometerConfig()->baro_hardware);
#endif
#ifdef USE_RANGEFINDER
rangefinderInit();
#endif
#ifdef USE_ADC_INTERNAL
adcInternalInit();
#endif
return gyroDetected;
}
Sensor createSensor(SensorType type, int port) {
static bool initIME = false;
Sensor retSen = {.type = type, .port = port};
switch (type) {
case SHAFT_ENCODER:
retSen.enc = encoderInit(port, port + 1, false);
if (DEBUG) {
print("encoder init");
if (retSen.enc == NULL) {
print(" FAILED");
}
getchar();
}
break;
case GYROSCOPE:
retSen.gyr = gyroInit(port, 0);
break;
case ULTRASONIC:
retSen.ult = ultrasonicInit(port, port + 1);
break;
case IME:
if (!initIME) {
printDebug("Init IME!");
// imeReinitialize();
initIME = true;
printDebug("Done");
}
break;
default:
break;
}
return retSen;
}
static msg_t sensorsRead (void*arg)
{
float acc_x, acc_y, acc_z;
float gyro_x, gyro_y, gyro_z;
uint8_t temp;
/*sensors initializing*/
gyroInit (&I2CD2, IMU01A_GYRO);
accInit (&I2CD2, IMU01A_ACC);
chprintf ((BaseSequentialStream *)&SD2, "Configuration done\n\r");
while (true)
{
/*gyroscope and temperature reading*/
gyroRead (&I2CD2, IMU01A_GYRO, &gyro_x, &gyro_y, &gyro_z);
tempRead (&I2CD2, IMU01A_GYRO, &temp);
/*accelerometer reading*/
accRead (&I2CD2, IMU01A_ACC, &acc_x, &acc_y, &acc_z);
/*printing out magnetometer values*/
chprintf ((BaseSequentialStream *)&SD2, "%f gX %f gY %f gZ ", gyro_x, gyro_y, gyro_z);
/*printing out accelerometer and temperature values*/
chprintf ((BaseSequentialStream *)&SD2, "%f aX %f aY %f aZ ", acc_x, acc_y, acc_z);
chprintf ((BaseSequentialStream *)&SD2, "%d T\n\r", temp);
}
}
//======================== compass ===================
uint8_t compassInit(void)
{ int ret1,ret2,ret3;
ret1=gyroInit();
ret2=accInit();
ret3=magInit();
//
return ret3;
}
void GyroSensor::setMultiplier(int multi)
{
if(sensorport != 13) {
gyro = gyroInit(sensorport, multi);
reset();
pollSensor();
}
}
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration----------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* Configure the system clock */
SystemClock_Config();
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_ADC1_Init();
MX_I2C1_Init();
MX_RTC_Init();
MX_SPI2_Init();
MX_TIM1_Init();
MX_TIM2_Init();
MX_TIM3_Init();
MX_TIM4_Init();
MX_USART3_UART_Init();
/* USER CODE BEGIN 2 */
encoderInit();
pwmInit();
// adcInit();
uartInit();
timInterruptInit();
gyroInit(GYROHIGH);
calibrateGyro();
// rotaryRight(800);
// HAL_Delay(500);
// rotaryLeft(800);
drive(VEL);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
static void calculations_heading(void *pvParameters)
{
UARTInit(3, 115200); /* baud rate setting */
printf("Maintask: Running\n");
CLKPWR_ConfigPPWR(CLKPWR_PCONP_PCGPIO, ENABLE);
spiInit();
gyroInit();
for (;;)
{
/*Block waiting for the semaphore to become available. */
if (xSemaphoreTake( xSemaphore, 0xffff ) == pdTRUE)
{
/* It is time to execute. */;
/* Turn the LED off if it was on, and on if it was off. */
LPC_GPIO1->FIOSET = (1 << 1);
static Bool initDone = TRUE;
if (initDone)
{
static uint16_t initIteration = 0;
if (initIteration++ == 33)//not 32 since first value is dummy =0,0
{
initDone = FALSE;
imuInit_2();
}
else
imuInit_1();
}
else
{
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
calculations_motor();
static uint16_t counter = 0;
counter++;
if (counter == 4)
{
counter = 0;
xSemaphoreGive(xSemaphoreTerminal);
}
}
LPC_GPIO1->FIOCLR = (1 << 1);
}
}
}
//Calibrate gyro and initialize PID controllers
void
pre_auton(){
//Allow gyro to settle and then init/calibrate (Takes a total of around 2 seconds)
delay(1100);
gyroInit(gyro, 1);
//These NEED to be tuned
pidInit(gyroAnglePid, 2, 0, 0.15, 2, 20.0); //No idea if these are any good, they need to be tuned a TON
pidInit(gyroRatePid, 10, 0, 0, 0, 360.00); //need to be tuned
}
GyroSensor::GyroSensor(int SensorPort, int multi)
{
if(checkAnalogPort(SensorPort))
sensorport = SensorPort;
else
sensorport = 13;
if(sensorport != 13)
gyro = gyroInit(sensorport, 0);
curval = 0;
multipier = 0;
}
/*
* Runs user initialization code. This function will be started in its own task
* with the default
* priority and stack size once when the robot is starting up. It is possible
* that the VEXnet
* communication link may not be fully established at this time, so reading from
* the VEX
* Joystick may fail.
*
* This function should initialize most sensors (gyro, encoders, ultrasonics),
* LCDs, global
* variables, and IMEs.
*
* This function must exit relatively promptly, or the operatorControl() and
* autonomous() tasks
* will not start. An autonomous mode selection menu like the pre_auton() in
* other environments
* can be implemented in this task if desired.
*/
void initialize() {
print("[Init] Initializing the robot\n");
// Set up the LCD and start it
print("[Init] Setting up the LCD\n");
lcdInitMenu(1, 3, 1); // Min 1, max 3, home 1
lcdSetUpdater(implUpdateLCD);
lcdSetMenuBack(implMenuBack);
lcdSetMenuNext(implMenuNext);
lcdSetCycles(true);
// Set up our drive
print("[Init] Setting up drive motors\n");
lcdSetText(uart2, 1, "Init drive...");
driveInit(MOTOR_DRIVE_FL, MOTOR_DRIVE_BL, MOTOR_DRIVE_FR, MOTOR_DRIVE_BR);
driveSetReverse(MOTOR_DRIVE_FL_REV, MOTOR_DRIVE_BL_REV, MOTOR_DRIVE_FR_REV,
MOTOR_DRIVE_BR_REV);
// enableSlew(15); // Set slew rate to 15
// Set up our autonomous to these modes
print("[Init] Setting up autonomous modes\n");
lcdSetText(uart2, 1, "Init auton...");
autonInit(4); // 3 auton modes
// Set up our gyroscope
print("[Init] Setting gyroscope\n");
lcdSetText(uart2, 1, "Init gyro...");
// To tune: 196*((360*rotations)/gyroValue)
gyro = gyroInit(ANALOG_GYRO, 190); // default is 196, this is after tune
// Set up our encoders
print("[Init] Setting up encoders\n");
lcdSetText(uart2, 1, "Init Encs...");
initDriveEncoders();
// initPidControl();
// Sets communication port for JINX data and start task to parse incoming
// messages.
print("[Init] Setting up JINX\n");
lcdSetText(uart2, 1, "Init JINX...");
initJINX(stdout);
delay(100);
taskCreate(JINXRun, TASK_DEFAULT_STACK_SIZE, NULL, (TASK_PRIORITY_DEFAULT));
delay(100);
// Done init
print("[Init] Finished, starting LCD menu\n");
lcdSetText(uart2, 1, "Init menu...");
lcdStartMenu();
// TODO Remove
hc05Init(1, false);
}
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
uint8_t accHardwareToUse,
uint8_t magHardwareToUse,
uint8_t baroHardwareToUse,
int16_t magDeclinationFromConfig,
uint8_t gyroLpf,
uint8_t gyroSyncDenominator)
{
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig);
UNUSED(mpuDetectionResult);
#endif
if (!detectGyro()) {
return false;
}
detectAcc(accHardwareToUse);
detectBaro(baroHardwareToUse);
// Now time to init things, acc first
if (sensors(SENSOR_ACC)) {
acc.acc_1G = 256; // set default
acc.init(&acc);
}
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation
gyro.init(gyroLpf);
gyroInit();
detectMag(magHardwareToUse);
reconfigureAlignment(sensorAlignmentConfig);
// FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
if (sensors(SENSOR_MAG)) {
// calculate magnetic declination
const int16_t deg = magDeclinationFromConfig / 100;
const int16_t min = magDeclinationFromConfig % 100;
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
} else {
magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
}
return true;
}
bool sensorsAutodetect(void)
{
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig);
UNUSED(mpuDetectionResult);
#endif
if (!detectGyro()) {
return false;
}
gyro.sampleFrequencyHz = gyroConfig()->gyro_sample_hz;
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init(&gyro, gyroConfig()->gyro_lpf);
gyroInit();
// the combination of LPF and GYRO_SAMPLE_HZ may be invalid for the gyro, update the configuration to use the sample frequency that was determined for the desired LPF.
gyroConfig()->gyro_sample_hz = gyro.sampleFrequencyHz;
if (detectAcc(sensorSelectionConfig()->acc_hardware)) {
acc.acc_1G = 256; // set default
acc.init(&acc);
}
#ifdef BARO
detectBaro(sensorSelectionConfig()->baro_hardware);
#endif
#ifdef MAG
if (detectMag(sensorSelectionConfig()->mag_hardware)) {
if (!compassInit()) {
sensorsClear(SENSOR_MAG);
}
}
#endif
reconfigureAlignment(sensorAlignmentConfig());
return true;
}
Drive initDrive(int leftMotor, int leftMotorInverted,
int rightMotor, int rightMotorInverted, int leftEncoderTopPort,
int leftEncoderBottomPort, int leftEncoderInverted,
int rightEncoderTopPort, int rightEncoderBottomPort,
int rightEncoderInverted, int gyroPort, int echoPort,
int pingPort)
{
PantherMotor left = initPantherMotor(leftMotor, leftMotorInverted);
PantherMotor right = initPantherMotor(rightMotor, rightMotorInverted);
Encoder leftEncoder = encoderInit(leftEncoderTopPort,
leftEncoderBottomPort, leftEncoderInverted);
Encoder rightEncoder = encoderInit(rightEncoderTopPort,
rightEncoderBottomPort, rightEncoderInverted);
Gyro gyro = gyroInit(gyroPort, 0);
Ultrasonic newSonar = ultrasonicInit(echoPort, pingPort);
Drive newDrive = {left, right, leftEncoder, rightEncoder, gyro, newSonar};
return newDrive;
}
//Initialize all sensors on the IMU board
void IMUinit() {
//Initialize the I2C bus
i2cInit();
//Disable interrupts while everything is being set up
CRITICAL_SECTION_START;
//Initialize the gyro
if(gyroInit(GYRO_RANGE_250DPS)) {
theFlags.gyroEnabled = TRUE;
//Set the pin as input
cbi(DDRE, DDE4);
//Disable the pullup on the relevant pin
cbi(PORTE, PORTE4);
//Attach the DRDY interrupt
sbi(EICRB, ISC40); //Writing 1 to ISC40 and ISC41 sets interrupt on rising edge
sbi(EICRB, ISC41);
sbi(EIMSK, INT4); //Setting INT4 in EIMSK enables the interrupt (if they are globally enabled (but they are cause i2cinit did that))
//Now wait for DRDY to go low and reenmable it to start everything off
while(inb(PINE) & _BV(PINE4)) {
; //Wait for the port to go low
}
gyroEnableDrdy();
} else {
#ifdef IMU_DEBUG
printf("Error intitializing the gyro!\r\n");
#endif
theFlags.gyroEnabled = FALSE;
}
//Intialize the accelerometer
if(accelInit()) {
theFlags.accelEnabled = TRUE;
//Set the pin as input
cbi(DDRD, DDD2);
//Disable the pullup on the relevant pin
cbi(PORTD, PORTD2);
//Attach the DRDY interrupt
sbi(EICRA, ISC20); //Writing 1 to ISC20 and ISC21 sets interrupt on rising edge
sbi(EICRA, ISC21);
sbi(EIMSK, INT2); //Setting INT2 in EIMSK enables the interrupt (if they are globally enabled (but they are cause i2cinit did that))
//Now wait for DRDY to go low and reenable it to start everything off
while(inb(PINB) & _BV(PINB2)) {
; //Wait for the port to go low
}
accelEnableDrdy();
} else {
#ifdef IMU_DEBUG
printf("Error intitializing the accelerometer!\r\n");
#endif
theFlags.accelEnabled = FALSE;
}
//Initialize the magnetometer
if(magInit()) {
theFlags.magEnabled = TRUE;
//Set the pin as input
cbi(DDRD, DDD3);
//Disable the pullup on the DRDY pin
cbi(PORTD, PORTD3);
//Attach the DRDY interrupt
sbi(EICRA, ISC30); //Writing 1 to ISC30 and ISC31 sets interrupt on rising edge
sbi(EICRA, ISC31);
sbi(EIMSK, INT3); //Setting INT3 in EIMSK enables the interrupt (if they are globally enabled (but they are cause i2cinit did that))
} else {
#ifdef IMU_DEBUG
printf("Error intitializing the magnetometer!\r\n");
#endif
theFlags.magEnabled = FALSE;
}
//Initialize the BMP180 pressure/temp sensor
if(bmpInit()) {
theFlags.bmpEnabled = TRUE;
//Attach the interrupt handler for the timer ticks
timerSetInterruptCallback( imuTimerTick );
//Start a temperature measurrement to set everything off
const u08 toSend[2] = {BMP180_REGISTER_CONTROL, BMP180_REGISTER_READTEMPCMD};
i2cMasterSendNI(BMP180_ADDRESS, 2, (u08*)&toSend);
myBmpState = TEMPERATURE_MEASURING;
bmpLastStateChange = millis();
} else {
#ifdef IMU_DEBUG
printf("Error intitializing the BMP180!\r\n");
#endif
theFlags.bmpEnabled = FALSE;
}
//Attach the custom stop handler after all sensors are initialized
i2cSetStopHandler( customStopHandler );
//Re-enable the interrupts
CRITICAL_SECTION_END;
}