task main() {
//Set up precision speed control (which, incidentally, uses PID to do it :D)
nMotorPIDSpeedCtrl[left] = mtrSpeedReg;
nMotorPIDSpeedCtrl[right] = mtrSpeedReg;
//Initialize PIDs.
PID leftPID,rightPID,accelPID;
initPID(leftPID,3.5,0,0);
initPID(rightPID,3.5,0,0);
initPID(accelPID,2,0,0);
INTR velIntr;
initIntr(velIntr);
//Tuning tips: http://robotics.stackexchange.com/questions/167/what-are-good-strategies-for-tuning-pid-loops
//Also, double PID loops: http://forum.arduino.cc/index.php?topic=197688.0
//PID for physical position, not just rotational? (need accel)
//Swag: https://www.youtube.com/watch?v=n_6p-1J551Y
//Maybe we should try a unisphere balancing robot :) https://www.youtube.com/watch?v=bI06lujiD7E
//Stands up and initiates gyro stuff.
initSweg();
//waitForStart();
//Initialize steering, raise arms... let's start!!
StartTask(steer);
servo[rearFlipper] = REAR_LIFT_RAISED;
servo[frontFlipper] = FRONT_LIFT_RAISED;
EndTimeSlice();
while(true) {
int ax,ay,az;
HTACreadAllAxes(accel, ax, ay, az);
float yvel = integrate(velIntr,ay);
if(abs(forwardsAngle) > 50) {//If it fell over, reset and redo.
reset(leftPID);
reset(rightPID);
initSweg();
servo[rearFlipper] = REAR_LIFT_RAISED;
servo[frontFlipper] = FRONT_LIFT_RAISED;
}
//Separate left and right PIDs to allow steering by NeutralAngle adjustment.
motor[left] = updatePID(leftPID, leftNeutral - forwardsAngle);
motor[right] = updatePID(rightPID, rightNeutral - forwardsAngle);
motor[left] = -motor[right];
wait1Msec(5);
}
}
void initAsservPosition()
{
/*
Step 1: Initialize the PID data structure, PID
*/
pos = getPosition();
kCoeffsl[0] = p;
kCoeffsl[1] = 0.049;
kCoeffsl[2] = 0.0;
kCoeffst[0] = p/2;
kCoeffst[1] = 0.1;
kCoeffst[2] = 0.0;
#ifndef PID_
initPID(&PIDl); /*Clear the controller history and the controller output */
setPIDCoeffs(&PIDl, kCoeffsl); /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */
setPIDMeas(&PIDl, pos->l);
initPID(&PIDt); /*Clear the controller history and the controller output */
setPIDCoeffs(&PIDt, kCoeffst); /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */
setPIDMeas(&PIDt, pos->T);
#else
initPID_(&PIDl); /*Clear the controller history and the controller output */
setPID_Coeffs(&PIDl, kCoeffsl); /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */
setPID_Meas(&PIDl, pos->l);
initPID_(&PIDt); /*Clear the controller history and the controller output */
setPID_Coeffs(&PIDt, kCoeffst); /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */
setPID_Meas(&PIDt, pos->t);
setPID_Overshoot(&PIDl,1);
setPID_OvershootVal(&PIDl, 1.);
setPID_Overshoot(&PIDt,1);
setPID_OvershootVal(&PIDt, 2.);
#endif
setConsignePIDlt(0.0,0.0);
}
void testCalibratePID(int speed_){
initPID(65535,38,0,380);
initEncoders();
regelverzoegerung=50;
PIDA_Testsollwert= speed_;
PIDB_Testsollwert= speed_;
PIDA_Activated=1;
PIDB_Activated=1;
}
void systemInit(void)
{
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
///////////////////////////////////
checkFirstTime(false);
readEEPROM();
if (eepromConfig.receiverType == SPEKTRUM)
checkSpektrumBind();
checkResetType();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
initMixer();
ledInit();
cliInit();
BLUE_LED_ON;
delay(20000); // 20 sec total delay for sensor stabilization - probably not long enough.....
adcInit();
batteryInit();
gpsInit();
i2cInit(I2C1);
i2cInit(I2C2);
pwmEscInit(eepromConfig.escPwmRate);
pwmServoInit(eepromConfig.servoPwmRate);
rxInit();
spiInit(SPI2);
spiInit(SPI3);
telemetryInit();
timingFunctionsInit();
initFirstOrderFilter();
initGPS();
initMax7456();
initPID();
GREEN_LED_ON;
initMPU6000();
initMag(HMC5883L_I2C);
initPressure(MS5611_I2C);
}
void initSwerveModule(int number, tMotor driveMotor, TServoIndex turnMotor, bool inverted, int turnZeroOffset)
{
initPID(modules[number].turnPID, 0.01, 0, 0);
modules[number].turnPID.target = 200;
modules[number].turnMotor = turnMotor;
modules[number].driveMotor = driveMotor;
modules[number].number = number;
modules[number].inverted = inverted;
modules[number].turnZeroOffset = turnZeroOffset;
int reading = HTSPBreadADC(proto, number, 8);
}
void initAsservPosition(void) {
/*
Step 1: Initialize the PID data structure, PID
*/
pos = getPosition();
kCoeffsgP[0] = 0.25;
kCoeffsgP[1] = 0.;
kCoeffsgP[2] = 1.;
kCoeffsdP[0] = .25;
kCoeffsdP[1] = 0.;
kCoeffsdP[2] = 1.;
#ifndef PID_
initPID(&PIDPd); /*Clear the controller history and the controller output */
setPIDCoeffs(&PIDPd, kCoeffs1P); /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */
setPIDMeas(&PIDPd, pos->p1);
initPID(&PIDPg); /*Clear the controller history and the controller output */
setPIDCoeffs(&PIDPg, kCoeffs2P); /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */
setPIDMeas(&PIDPg, pos->p2);
#else
initPID_(&PIDPd); /*Clear the controller history and the controller output */
setPID_Coeffs(&PIDPd, kCoeffsdP); /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */
setPID_Meas(&PIDPd, pos->p1);
initPID_(&PIDPg); /*Clear the controller history and the controller output */
setPID_Coeffs(&PIDPg, kCoeffsgP); /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */
setPID_Meas(&PIDPg, pos->p2);
setPID_Overshoot(&PIDPd,1);
setPID_OvershootVal(&PIDPd, 10.0);
setPID_Overshoot(&PIDPg,1);
setPID_OvershootVal(&PIDPg, 10.0);
#endif
setConsignePIDgd(0.0,0.0);
}
void initPID(PID& pid, double p, double i, double d, double s_max) {
initPID(pid, p, i, d);
pid.s_max = s_max;
}
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();
}
void systemInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
uint8_t i;
gpio_config_t 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(false, false);
readEEPROM();
initMixer();
pwmOutputConfig.escPwmRate = systemConfig.escPwmRate;
pwmOutputConfig.servoPwmRate = systemConfig.servoPwmRate;
//i2cInit(SENSOR_I2C);
pwmInputInit();
pwmOutputInit(&pwmOutputConfig);
uartInit(115200);
//delay(10000); // 10 sec delay for sensor stabilization - probably not long enough.....
//initAccel();
//initGyro();
//initMag();
adcInit();
//initPressure(); // no pressure sensor
initPID();
}
void operatorControl() {
//autonomous();
unsigned long prevWakeupTime = millis();
// PID controller variable
struct pid_dat arm_pid;
initPID(&arm_pid, 2, 0.5, 0.02);
while (1) {
/* Stores which joysticks are connected
* 0 - None
* 1 - Only Joystick 1
* 2 - Only Joystick 2
* 3 - Both Josticks 1 and 2
*/
int joystickStatus = 0;
if (isJoystickConnected(1))
joystickStatus |= 1;
if (isJoystickConnected(2))
joystickStatus |= 2;
// Local Variables
int moveX = 0, moveY = 0, rotate = 0, arm = 0, liftSpeed = 0;
int L, R, C;
bool liftUp = 0, liftDw = 0, supUp = 0, supDw = 0, liftCenter = 0, liftLower = 0;
// Subteam Definition
int team1 = digitalRead(1);
// Get Joystick Values based on Status
if (joystickStatus == 3) {
// Driving
moveX = joystickGetAnalog(1, JOY_LX); // Movement
moveY = joystickGetAnalog(1, JOY_LY); // Movement
rotate = joystickGetAnalog(1, JOY_RX); // Rotate
arm = joystickGetAnalog(2, JOY_RY);
// Support
supUp = joystickGetDigital(1, JOY_LBUM, JOY_DOWN);
supDw = joystickGetDigital(1, JOY_LBUM, JOY_UP);
// Both joysticks connected, reference as 1 or 2
liftUp = joystickGetDigital(1, JOY_RBUM, JOY_DOWN);
liftDw = joystickGetDigital(1, JOY_RBUM, JOY_UP);
liftCenter = joystickGetDigital(2, JOY_RBUM, JOY_UP);
liftLower = joystickGetDigital(2, JOY_RBUM, JOY_DOWN);
} else {
// Driving
moveX = joystickGetAnalog(joystickStatus, JOY_LX); // Movement
moveY = joystickGetAnalog(joystickStatus, JOY_LY); // Movement
rotate = joystickGetAnalog(joystickStatus, JOY_RX); // Rotate
// Support
supUp = joystickGetDigital(joystickStatus, JOY_LBUM, JOY_DOWN);
supDw = joystickGetDigital(joystickStatus, JOY_LBUM, JOY_UP);
// Lift
liftUp = joystickGetDigital(joystickStatus, JOY_RBUM, JOY_DOWN);
liftDw = joystickGetDigital(joystickStatus, JOY_RBUM, JOY_UP);
}
// Driving
L = CAP(moveY + rotate, RANGE_MAX);
R = CAP(moveY - rotate, RANGE_MAX);
C = CAP(moveX, RANGE_MAX);
motorSet(MC_WHEEL_L, team1 == ON ? L : L*2/3);
motorSet(MC_WHEEL_R, team1 == ON ? -R : -R*2/3);
motorSet(MC_WHEEL_M, team1 == ON ? -C : -C*2/3);
// Support
if (supDw)
motorSet(MC_SUPPORT, team1 == ON ? -32 : 32);
else if (supUp)
motorSet(MC_SUPPORT, team1 == ON ? 32 : -32);
else
motorSet(MC_SUPPORT, 0);
if (team1 == ON) {
double loc = ((MAP_INPUT(arm) + 1.0) / 2.0) * 0.9 - 0.8;
if(liftCenter) {
loc = -0.35;
} else if(liftLower) {
loc = -0.45;
}
liftSpeed = -MAP_OUTPUT(computePID(loc, MAP_POT(analogRead(1)), &arm_pid ));
}
if (liftUp)
liftSpeed = LIFTSPEED;
else if (liftDw)
liftSpeed = -LIFTSPEED;
if(team1 != ON) {
liftSpeed = -liftSpeed;
}
// Lift
motorSet(MC_LIFT_BL, liftSpeed);
motorSet(MC_LIFT_ML, liftSpeed);
motorSet(MC_LIFT_TL, liftSpeed);
motorSet(MC_LIFT_BR, -liftSpeed);
motorSet(MC_LIFT_MR, -liftSpeed);
motorSet(MC_LIFT_TR, -liftSpeed);
// Motors can only be updated once every 20ms, therefore updating at twice the rate for better response time
taskDelayUntil(&prevWakeupTime, 10);
}
}
void systemInit(void)
{
RCC_ClocksTypeDef rccClocks;
///////////////////////////////////
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB |
RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO |
RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8 |
RCC_APB2Periph_ADC1, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 |
RCC_APB1Periph_TIM5 | RCC_APB1Periph_TIM6 |
RCC_APB1Periph_I2C2, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
#ifdef _DTIMING
timingSetup();
#endif
///////////////////////////////////////////////////////////////////////////
checkFirstTime(false);
readEEPROM();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
pwmMotorDriverInit();
cliInit();
gpioInit();
adcInit();
LED2_ON;
delay(10000); // 10 seconds of 20 second delay for sensor stabilization
if (GetVCPConnectMode() != eVCPConnectReset)
{
cliPrintF("\r\nUSB startup delay...\r\n");
delay(3000);
if (GetVCPConnectMode() == eVCPConnectData)
{
cliPrintF("\r\nBGC32 firmware starting up, USB connected...\r\n");
}
}
else
{
cliPrintF("\r\nDelaying for usb/serial driver to settle\r\n");
delay(3000);
cliPrintF("\r\nBGC32 firmware starting up, serial active...\r\n");
}
#ifdef __VERSION__
cliPrintF("\ngcc version " __VERSION__ "\n");
#endif
cliPrintF("BGC32 Firmware V%s, Build Date " __DATE__ " "__TIME__" \n", __BGC32_VERSION);
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
cliPrintF("\nRunning on external HSE clock....\n");
}
else
{
cliPrintF("\nERROR: Running on internal HSI clock....\n");
}
RCC_GetClocksFreq(&rccClocks);
cliPrintF("\nADCCLK-> %2d MHz\n", rccClocks.ADCCLK_Frequency / 1000000);
cliPrintF( "HCLK-> %2d MHz\n", rccClocks.HCLK_Frequency / 1000000);
cliPrintF( "PCLK1-> %2d MHz\n", rccClocks.PCLK1_Frequency / 1000000);
cliPrintF( "PCLK2-> %2d MHz\n", rccClocks.PCLK2_Frequency / 1000000);
cliPrintF( "SYSCLK-> %2d MHz\n\n", rccClocks.SYSCLK_Frequency / 1000000);
delay(10000); // Remaining 10 seconds of 20 second delay for sensor stabilization - probably not long enough..
LED1_ON;
i2cInit(I2C2);
rcInit();
timingFunctionsInit();
BKPInit();
initFirstOrderFilter();
initPID();
initSinArray();
orientIMU();
initMPU6050();
// initMag();
}
void systemInit(void)
{
RCC_ClocksTypeDef rccClocks;
///////////////////////////////////
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
// Turn on peripherial clocks
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_ADC12, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); // USART1, USART2
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE); // ADC2
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); // PPM RX
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); // PWM ESC Out 1 & 2
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); // PWM ESC Out 5 & 6
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); // PWM Servo Out 1, 2, 3, & 4
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); // 500 Hz dt Counter
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); // 100 Hz dt Counter
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM15, ENABLE); // PWM ESC Out 3 & 4
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM16, ENABLE); // RangeFinder PWM
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM17, ENABLE); // Spektrum Frame Sync
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); // Telemetry
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); // GPS
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); // Spektrum RX
///////////////////////////////////////////////////////////////////////////
spiInit(SPI2);
///////////////////////////////////
checkSensorEEPROM(false);
checkSystemEEPROM(false);
readSensorEEPROM();
readSystemEEPROM();
///////////////////////////////////
if (systemConfig.receiverType == SPEKTRUM)
checkSpektrumBind();
///////////////////////////////////
checkResetType();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
///////////////////////////////////
gpsPortClearBuffer = &uart2ClearBuffer;
gpsPortNumCharsAvailable = &uart2NumCharsAvailable;
gpsPortPrintBinary = &uart2PrintBinary;
gpsPortRead = &uart2Read;
telemPortAvailable = &uart1Available;
telemPortPrint = &uart1Print;
telemPortPrintF = &uart1PrintF;
telemPortRead = &uart1Read;
///////////////////////////////////
initMixer();
usbInit();
gpioInit();
uart1Init();
uart2Init();
LED0_OFF;
delay(10000); // 10 seconds of 20 second delay for sensor stabilization
checkUsbActive();
///////////////////////////////////
#ifdef __VERSION__
cliPortPrintF("\ngcc version " __VERSION__ "\n");
#endif
cliPortPrintF("\nFF32mini Firmware V%s, Build Date " __DATE__ " "__TIME__" \n", __FF32MINI_VERSION);
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
cliPortPrint("\nRunning on external HSE clock....\n");
}
else
{
cliPortPrint("\nERROR: Running on internal HSI clock....\n");
}
RCC_GetClocksFreq(&rccClocks);
cliPortPrintF("\nHCLK-> %2d MHz\n", rccClocks.HCLK_Frequency / 1000000);
cliPortPrintF( "PCLK1-> %2d MHz\n", rccClocks.PCLK1_Frequency / 1000000);
cliPortPrintF( "PCLK2-> %2d MHz\n", rccClocks.PCLK2_Frequency / 1000000);
cliPortPrintF( "SYSCLK-> %2d MHz\n\n", rccClocks.SYSCLK_Frequency / 1000000);
if (systemConfig.receiverType == PPM)
cliPortPrint("Using PPM Receiver....\n\n");
else
cliPortPrint("Using Spektrum Satellite Receiver....\n\n");
initUBLOX();
delay(10000); // Remaining 10 seconds of 20 second delay for sensor stabilization - probably not long enough..
///////////////////////////////////
adcInit();
aglInit();
pwmServoInit();
if (systemConfig.receiverType == SPEKTRUM)
spektrumInit();
else
ppmRxInit();
timingFunctionsInit();
batteryInit();
initFirstOrderFilter();
initMavlink();
initPID();
LED0_ON;
initMPU6000();
initMag();
initPressure();
}
void drivetrainInit()
{
initPID(drivetrainPID, DRIVETRAIN_P_CONSTANT, DRIVETRAIN_I_CONSTANT, DRIVETRAIN_D_CONSTANT);
initPID(drivetrainTurnPID, DRIVETRAIN_TURN_P_CONSTANT, DRIVETRAIN_TURN_I_CONSTANT, DRIVETRAIN_TURN_D_CONSTANT);
resetDrivetrainDistance();
}
void systemInit(void)
{
RCC_ClocksTypeDef rccClocks;
///////////////////////////////////
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
// Turn on peripherial clcoks
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC2, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3, 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_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM10, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM11, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
///////////////////////////////////
checkFirstTime(false);
readEEPROM();
if (eepromConfig.receiverType == SPEKTRUM)
checkSpektrumBind();
checkResetType();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
///////////////////////////////////
gpsPortClearBuffer = &uart2ClearBuffer;
gpsPortNumCharsAvailable = &uart2NumCharsAvailable;
gpsPortPrintBinary = &uart2PrintBinary;
gpsPortRead = &uart2Read;
telemPortAvailable = &uart1Available;
telemPortPrint = &uart1Print;
telemPortPrintF = &uart1PrintF;
telemPortRead = &uart1Read;
///////////////////////////////////
initMixer();
usbInit();
ledInit();
uart1Init();
uart2Init();
BLUE_LED_ON;
///////////////////////////////////
delay(10000); // 10 seconds of 20 second delay for sensor stabilization
checkUsbActive();
#ifdef __VERSION__
cliPortPrintF("\ngcc version " __VERSION__ "\n");
#endif
cliPortPrintF("\nAQ32Plus Firmware V%s, Build Date " __DATE__ " "__TIME__" \n", __AQ32PLUS_VERSION);
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
cliPortPrint("\nRunning on external HSE clock....\n");
}
else
{
cliPortPrint("\nERROR: Running on internal HSI clock....\n");
}
RCC_GetClocksFreq(&rccClocks);
cliPortPrintF("\nHCLK-> %3d MHz\n", rccClocks.HCLK_Frequency / 1000000);
cliPortPrintF( "PCLK1-> %3d MHz\n", rccClocks.PCLK1_Frequency / 1000000);
cliPortPrintF( "PCLK2-> %3d MHz\n", rccClocks.PCLK2_Frequency / 1000000);
cliPortPrintF( "SYSCLK-> %3d MHz\n\n", rccClocks.SYSCLK_Frequency / 1000000);
initUBLOX();
delay(10000); // Remaining 10 seconds of 20 second delay for sensor stabilization - probably not long enough..
///////////////////////////////////
adcInit();
i2cInit(I2C1);
i2cInit(I2C2);
pwmServoInit();
rxInit();
spiInit(SPI2);
spiInit(SPI3);
timingFunctionsInit();
batteryInit();
initFirstOrderFilter();
initMavlink();
initMax7456();
initPID();
GREEN_LED_ON;
initMPU6000();
initMag();
initPressure();
}
void systemInit(void)
{
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
// Turn on peripherial clocks
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_ADC12, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); // USART1
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE); // ADC2
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); // PWM Servo Out 1 & 2
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); // PWM ESC Out 3 & 4
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); // PWM ESC Out 5,6,7, & 8
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); // 500 Hz dt Counter
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); // 100 Hz dt Counter
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM15, ENABLE); // PWM ESC Out 1 & 2
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM16, ENABLE); // PPM RX
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM17, ENABLE); // Spektrum Frame Sync
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); // Telemetry
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); // Spektrum RX
///////////////////////////////////////////////////////////////////////////
checkFirstTime(false);
readEEPROM();
if (eepromConfig.receiverType == SPEKTRUM)
checkSpektrumBind();
checkResetType();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
initMixer();
cliInit();
gpioInit();
telemetryInit();
adcInit();
LED0_OFF;
delay(20000); // 20 sec total delay for sensor stabilization - probably not long enough.....
LED0_ON;
batteryInit();
pwmServoInit(eepromConfig.servoPwmRate);
if (eepromConfig.receiverType == SPEKTRUM)
spektrumInit();
else
ppmRxInit();
spiInit(SPI2);
timingFunctionsInit();
initFirstOrderFilter();
initPID();
initMPU6000();
initMag();
initPressure();
}
void main(void) {
int num_behaviors=6; // number of behaviors
U32 robot_sched_duration=100; // minimum time spent for each main loop (in ms)
char *title="Tribot"; // program title to display
U32 max_iterations=65536; // maximum number of iterations of main loop
U32 sched_tick=0; // store system timer for robot sleep
// Tone Control Constants
U32 tone_enabled_freq=2500; // Hz
U32 tone_silenced_freq=0; // Hz
U32 tone_enabled_dur=100; // ms
U32 tone_obstacle_freq=750; // Hz
// wheel control constants
S8 stop_speed=0;
S8 fwd_speed=30;
S8 fastrot_speed=40;
S8 slowrot_speed=20;
S8 seek_speed=30;
// Idle Behavior Variables
bool tone_active=FALSE;
U32 tone_timestamp=0;
// Follow Line Behavior Variables
U8 sampleSize=5;
SensorReading light_readings[sampleSize];
SensorReading light_min;
SensorReading light_max;
// Open-Claws Behavior and Grasp-Object Behavior Variables
SensorReading touched;
U32 claw_timestamp=0;
U32 claw_oldtstamp=0;
U32 claw_position=0;
U32 claw_oldpos=0;
bool claw_closed;
U8 claw_movement;
// Claw Control Constants
// Open and Close Speeds are directional (signed)
// WARNING: Be conservative with the Open and Close Speeds, since we
// run the motors until it hits the limiters and the Tachometer stops changing.
// Using too high a speed may pop the claw assembly!
S8 open_speed=10;
S8 close_speed=-20;
// Move-Object Behavior Variables
SensorReading sound_levels[sampleSize];
SensorReading sound_min;
SensorReading sound_max;
U8 sound_following_state=0;
U8 sound_level_trigger=0;
// Avoid Object behavior variables
bool obstacle_detected=FALSE;
bool obstacle_tone_active=FALSE;
U32 obstacle_tone_timestamp=0;
struct PID_Control pid;
SensorReading distance;
distance.reading.analog=0;
// Actuator Port Assignments
Actuators actuators;
setActuators(&actuators, ARM, RIGHT_WHEEL, LEFT_WHEEL );
// Sensor Port Assignments and configure
Sensor lightSensor, touchSensor, soundSensor, ulsSensor;
//current robot actuator state
ActuatorState current_actuation={MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, TONE_INHIBITED, TONE_INHIBITED};
//previous robot actuator state
ActuatorState previous_actuation={MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, TONE_INHIBITED, TONE_INHIBITED};
//behavior_actuations
ActuatorState behavior_actuations[num_behaviors];
//array of available states
ActuatorState availableStates[16]=
{
//bbr_behavior_inhibited
{MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, TONE_INHIBITED, TONE_INHIBITED},
//bbr_idle_on
{MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, tone_enabled_freq, tone_enabled_dur},
//bbr_idle_off
{MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, tone_silenced_freq, tone_enabled_dur},
//bbr_follow_line_black
{MOTOR_INHIBITED_BYTE, FALSE, fwd_speed, FALSE, fwd_speed, FALSE, ROBOT_FWD, tone_silenced_freq, robot_sched_duration},
//bbr_follow_line_edge
{MOTOR_INHIBITED_BYTE, FALSE, slowrot_speed, FALSE, stop_speed, FALSE, ROBOT_CCW, tone_silenced_freq, robot_sched_duration},
//bbr_follow_line_white
{MOTOR_INHIBITED_BYTE, FALSE, stop_speed, FALSE, fastrot_speed, FALSE, ROBOT_CW, tone_silenced_freq, robot_sched_duration},
// Open Claws
{open_speed, FALSE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration},
// Close Claws (Grasp Object)
{close_speed, FALSE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration},
// Unlock Claws
{stop_speed, FALSE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration},
// Lock Claws
{stop_speed, TRUE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration},
// Move-Object Behavior Actuation Outputs
// idle
{close_speed, TRUE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, TONE_INHIBITED, TONE_INHIBITED},
// waiting
{close_speed, TRUE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration},
// seeking
{close_speed, TRUE, seek_speed, TRUE, stop_speed, TRUE, ROBOT_SEEK, tone_silenced_freq, robot_sched_duration},
// follow
{close_speed, TRUE, fwd_speed, TRUE, fwd_speed, TRUE, ROBOT_FWD, tone_silenced_freq, robot_sched_duration},
// Avoid-Obstacle Behavior Actuation Outputs
// avoid obstacle template, tone on
{MOTOR_INHIBITED_BYTE, TRUE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_AVOID, tone_obstacle_freq, tone_enabled_dur},
// avoid obstacle template, tone off
{MOTOR_INHIBITED_BYTE, TRUE, MOTOR_INHIBITED_BYTE, TRUE, stop_speed, TRUE, ROBOT_AVOID, tone_silenced_freq, tone_enabled_dur}
};
//
// program initialization
//
nx_proginit();
nx_progtitle(title);
nx_systick_wait_ms(1000);
// configure sensors
configureSensor(&touchSensor, TOUCH, ONE);
configureSensor(&soundSensor, SOUND, TWO);
configureSensor(&ulsSensor, RADAR, THREE);
configureSensor(&lightSensor, LIGHT, FOUR);
light_led_enable(&lightSensor);
// Open-Claws Behavior and Grasp-Object Behavior initialization
touched.reading.touched=FALSE;
// force claw opening on init via Open-Claws behavior
claw_closed=TRUE; // claw closed boolean
claw_oldpos=0; // Initialize Old Position = 0 (default initial position after nx__motors_init)
claw_oldtstamp=0; // Initialize Claw Position Old Timestamp = 0
claw_movement= CLAW_STOPPED;
// Avoid-Obstacle Behavior initialization
distance.reading.analog=RADAR_DIST_ERR; // initialize to unknown distance
obstacle_tone_active=FALSE;
obstacle_detected=FALSE;
// PID Configuration Parameters
// FIXME: Need to be changed to actual values
// KP, KI, KD have the range of 16-bit integer values [MIN_SHORT,MAX_SHORT]
// This is multiplied by alpha to give 32-bit resolution values
// alphaKP, alphaKI, alphaKD BEFORE initializing the PID Controller
// The value of alpha is 2^SCALING_SHIFT, where SCALING_SHIFT is 16
// which gives alpha = 65536
//
// Applying Kc = 3, T = 100 ms, Pc = 1 s, alpha = 65536
//#define KP 127795 // alphaKP = alpha x 0.65 x Kc = 127795
//#define KI 25559 // alphaKI = alpha x Kp x T / (0.5 x Pc) = 25559
//#define KD 153354 // alphaKD = alpha x Kp x 0.12 x Pc / T = 153354
//#define STEADY_STATE_THRESHOLD 1 // 0 = disabled, !0 = Stop PID Controller after x iterations
// init pid
initPID(&pid, 127795, 25559, 153354, 1);
// initialize current tick
sched_tick=nx_systick_get_ms();
// initialize behavior_actuations
initActuatorStates(behavior_actuations, num_behaviors);
while(max_iterations--)
{
collect_samples(sampleSize, &lightSensor, light_readings, &soundSensor, sound_levels, NULL, NULL, NULL, NULL);
calc_min_max(light_readings, sampleSize, &light_min, &light_max);
calc_min_max(sound_levels, sampleSize, &sound_min, &sound_max);
getSensorReading(&touchSensor, &touched); // concerned about touched or not
getSensorReading(&ulsSensor, &distance); // concerned about range in cm
get_claw_status(actuators, &claw_position, &claw_timestamp);
// turn off light to save energy if follow line is not supposed to work
if (touched.reading.touched==FALSE)
{
light_led_enable(&lightSensor);
}
else
{
light_led_disable(&lightSensor);
}
// dispatcher
bbr_idle(&tone_timestamp, &tone_active,
behavior_actuations, availableStates);
bbr_follow_line(&light_min, &light_max,
behavior_actuations, availableStates);
bbr_open_claws(&claw_movement, &touched, &claw_closed, &claw_timestamp, &claw_oldtstamp,
&claw_position, &claw_oldpos, ¤t_actuation,&actuators,
behavior_actuations, availableStates);
bbr_grasp_object(&claw_movement, &touched, &claw_closed, &claw_timestamp, &claw_oldtstamp,
&claw_position, &claw_oldpos, ¤t_actuation, &actuators,
behavior_actuations, availableStates);
bbr_move_object(&sound_min, &sound_max, &touched, claw_closed, &sound_following_state,
&sound_level_trigger,
behavior_actuations, availableStates);
bbr_avoid_obstacle(&obstacle_detected, &distance, &obstacle_tone_timestamp, &obstacle_tone_active, &pid, &actuators,
behavior_actuations, availableStates);
// arbiters
arbiters(num_behaviors, &actuators, ¤t_actuation, behavior_actuations);
// actuators
controllers(&actuators, ¤t_actuation, &previous_actuation);
sleep_robot(&sched_tick, robot_sched_duration);
}
nx_progshutdown();
}
