Exemple #1
0
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;
	
	
	
}
Exemple #4
0
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);
}
Exemple #5
0
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);
}
Exemple #7
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);
	}

}
Exemple #11
0
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();
}
Exemple #12
0
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();
}
Exemple #14
0
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();
}
Exemple #15
0
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();
}
Exemple #16
0
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, &current_actuation,&actuators,
						behavior_actuations, availableStates);

		bbr_grasp_object(&claw_movement, &touched, &claw_closed, &claw_timestamp, &claw_oldtstamp,
				&claw_position, &claw_oldpos, &current_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, &current_actuation, behavior_actuations);

		// actuators
		controllers(&actuators, &current_actuation, &previous_actuation);


		sleep_robot(&sched_tick, robot_sched_duration);

	}



	nx_progshutdown();
}