void moveDistance(int distance) { motorInit(); // Start PWM process. Period 1 ms, Freq 1 kHz pwm_start(basepwm); // Turn motors counterclockwise for 3 s. if(distance >=0){ high(M1forward); high(M2forward); } else { high(M1reverse); high(M2reverse); distance=abs(distance); } pwm_set(M1enable, 0, 1000); pwm_set(M2enable, 1, 1000); pause(distance); // Stop again pwm_set(M1enable, 0, 0); pwm_set(M2enable, 1, 0); // End the PWM process pwm_stop(); }
int main(void) { xyInit(); pidInit(); motorInit(); orientationInit(); debugPrint("Initialized Hardware"); addTask(&flightTask); addTask(&statusTask); addMenuCommand('m', motorToggleString, &motorToggle); addMenuCommand('w', motorForwardString, &motorForward); addMenuCommand('a', motorLeftString, &motorLeft); addMenuCommand('s', motorBackwardString, &motorBackward); addMenuCommand('d', motorRightString, &motorRight); addMenuCommand('x', motorUpString, &motorUp); addMenuCommand('y', motorDownString, &motorDown); addMenuCommand('p', controlToggleString, &controlToggle); addMenuCommand('n', parameterChangeString, ¶meterChange); addMenuCommand('z', zeroString, &zeroOrientation); addMenuCommand('o', silentString, &silent); addMenuCommand('r', sensorString, &printRaw); xyLed(LED_RED, LED_OFF); xyLed(LED_GREEN, LED_ON); debugPrint("Starting Tasks"); for(;;) { tasks(); } return 0; }
void moveMotorT(int left, int right, int time) { motorInit(); // Start PWM process. Period 1 ms, Freq 1 kHz pwm_start(basepwm); if(left >=0){ high(M1forward); } else { high(M1reverse); left=abs(left); } if(right >=0){ high(M2forward); } else { high(M2reverse); right=abs(right); } pwm_set(M1enable, 0, left); pwm_set(M2enable, 1, right); pause(time); // Stop again pwm_set(M1enable, 0, 0); pwm_set(M2enable, 1, 0); // End the PWM process pwm_stop(); }
void ZeroPi::slotSetup(int slot, int fun){ if(fun==SLOT_STEPPER){ stepperInit(slot); }else if(fun==SLOT_MOTOR){ motorInit(slot); } }
void initialize() { servoInit(); motorInit(&desiredEncVals); //Initialize to zeroes memset(&desiredMotorVals, 0, sizeof(desiredMotorVals)); memset(&desiredEncVals, 0, sizeof(desiredEncVals)); clearDebugStream(); }
//perform all initialization void initialize() { //configure BTN1 as an input cbi(DDRD, 4); //enable pullup for BTN1 sbi(PORTD, 4); //configure LED as an output sbi(DDRG, 2); //configure 74LS374 (D Flip-Flop) clock pin as an output sbi(DDRD, 5); //configure LCD/Servo bus on port C as an output DDRC = 0xFF; #if USE_LCD == 1 //initialize LCD lcdInit(); #endif #if USE_I2C == 1 //configure I2C clock rate i2cInit(); #endif #if USE_MOTOR0 == 1 || USE_MOTOR1 == 1 //initialize enabled motors motorInit(); #endif #if NUM_SERVOS > 0 //initialize servos servoInit(); #endif #if USE_ADC == 1 //initialize ADC adcInit(); #endif #if USE_UART0 == 1 //initialize UART0 uart0Init(); #endif #if USE_UART1 == 1 //initialize UART1 uart1Init(); #endif #if USE_RTC == 1 //initialize RTC rtcInit(); #endif }
void initialize() { clearDebugStream(); writeDebugStream("This is JoyRecord\n"); //Initialize to zeroes memset(&desiredMotorVals, 0, sizeof(desiredMotorVals)); memset(&desiredEncVals, 0, sizeof(desiredEncVals)); motorInit(&desiredEncVals); servoInit(); }
void stopMotors() { motorInit(); // Stop pwm_set(M1enable, 0, 0); pwm_set(M2enable, 1, 0); // End the PWM process pwm_stop(); }
int main(int argc, char *argv[]) { motor_t rightMotor = { &PORTB, &DDRB, PORTB7, // PWM &PORTC, &DDRC, PORTC1, // Direction &OCR0A // Top value }; motor_t leftMotor = { &PORTD, &DDRD, PORTD0, // PWM &PORTB, &DDRB, PORTB0, // Direction &OCR0B // Top value }; //uint8_t sonarDistance = 0; cli(); NO_CLK_PRESCALE(); //uart_init(); radio_init(HOV1_ADDRESS, RECEIVE_MODE); sonar_init(); motorInit(&rightMotor); motorInit(&leftMotor); pwmInit(); sei(); stop = Event_Init(); sendPing(); while(!receivedInit); Task_Create((void*)(&motor_task),0, PERIODIC, MOTOR); Task_Create((void*)(&fire_sonar),0, PERIODIC, FIRE); Task_Create((void*)(&listen_sonar),0,PERIODIC, LISTEN); Task_Create((void*)(&sendRadio),0,PERIODIC, RADIO); Task_Create((void*)(&stopSystem), 0,SYSTEM, STOP); setMotorDirection(&rightMotor, FORWARD); setMotorDirection(&leftMotor, FORWARD); return 0; }
int main() { int wait; // Set up port for start switch lcdInit(); // Set up motors and encoders motorInit(); encodersInit(); servoInit(); // Set up IRs gp2d12Init(PIN_HEAD_IR); digitalSetDirection(PIN_IR,AVRRA_INPUT); digitalSetData(PIN_IR,AVRRA_LOW); // Set up OP599A - analog already initialized digitalSetDirection(PIN_PHOTO,AVRRA_INPUT); digitalSetData(PIN_PHOTO,AVRRA_LOW); // Set up fan & test digitalSetData(PIN_FAN, AVRRA_LOW); digitalSetDirection(PIN_FAN, AVRRA_OUTPUT); digitalSetData(PIN_FAN, AVRRA_HIGH); delayms(25); digitalSetData(PIN_FAN, AVRRA_LOW); lcdPrint("Wait... "); wait = digitalGetData(PIN_START); while(wait) { // standard delay .. delayms(10); wait = digitalGetData(PIN_START); } // Start our map mapInit(); lcdClear(); PrintHeading(lheading,nStart); sei(); // Run Behaviors while(1) { // update odometer while(rCount > COUNTS_CM(1) ) { // odometer is in CM odometer = odometer - 1; rCount = rCount - COUNTS_CM(1); } // now run behaviors if(arbitrate()>0) { // let motors wind down delayms(500); clearCounters; plan(); clearCounters; } } }
void InitAll() { // int i; // uint8 buffer_in[16]={0}; //uint8 buffer_out[16]={0}; CONSOLE_USART_INIT(); console_send("\r\n Motors device start\r"); LED_INIT(); I2C_INIT(); ReadConfig(); /* #ifdef EEPROM if (ReadConfig()==TRUE) console_send("\nEEPROM start\r"); else { console_send("\nEEPROM is not connect \r\n parameters is enabled by default\r"); SettingsDefault(); } #else SettingsDefault(); CheckAndWriteVersion(); PrintVersion(bufer_cons_out); console_send(bufer_cons_out); #endif */ (SysTick_Config(SystemCoreClock / 16)); motorInitTimer(); motorInitGpio(); motorInit(); motorTest(); motorSettings(); WIZ_GPIO_Install(); WIZ_Config(); console_send("\nWIZNET start\r\n>"); }
/* * Application entry point. */ int main(void) { /* * Shell thread */ Thread *shelltp = NULL; /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ halInit(); chSysInit(); /* * Activate custom stuff */ // mypwmInit(); // myADCinit(); // mySPIinit(); motorInit(); /* * Activates the USB driver and then the USB bus pull-up on D+. */ // myUSBinit(); /* * Main loop, does nothing except spawn a shell when the old one was terminated */ while (TRUE) { setMotorData(1300,1490); if (!shelltp && isUsbActive()) { shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO); } else if (chThdTerminated(shelltp)) { chThdRelease(shelltp); /* Recovers memory of the previous shell. */ shelltp = NULL; /* Triggers spawning of a new shell. */ } chThdSleepMilliseconds(1000); } }
/* * main program * */ void main(void) { initPorts(); // Port init initTimer(); // Timer init ifrRxFrontInit(); // Infrared init motorInit(); // Motor init EnableInterrupts; // Interrupts enable for(;;) { int8 speed = 0; switch (ifrRxFrontGetKey()) { case 'S': // stop motorSetPWMLeft(0); motorSetPWMRight(0); break; // @ToDo complete cases of the switch statement case 'W': // forward if ((motorGetPWMLeft() != 0) && (motorGetPWMRight() != 0)) { motorSetPWMLeft(80); motorSetPWMRight(80); } else { motorIncrementPWMLeft(5); motorIncrementPWMRight(5); } break; case 'T': // backward if ((motorGetPWMLeft() != 60) && (motorGetPWMRight() != 60)) { motorSetPWMLeft(-80); motorSetPWMRight(-80); } else { motorIncrementPWMLeft(-5); motorIncrementPWMRight(-5); } break; case '+': // left motorIncrementPWMLeft(-1); motorIncrementPWMRight(1); break; case '-': // right motorIncrementPWMLeft(1); motorIncrementPWMRight(-1); break; default: break; } } }
void userInit(void) { delayInit(72); uartxInit(); rgbLedInit(); ledGpioInit(); rgbKeyGpioInit(); motorInit(); dht11Init(); irInit(); watchdogInit(2); //5,625看门狗复位时间2s memset((uint8_t*)&reportData, 0, sizeof(gizwitsReport_t)); reportData.devStatus.Motor_Speed = protocolExchangeBytes(Y2X(MOTOR_SPEED_RATIO,MOTOR_SPEED_ADDITION,MOTOR_SPEED_DEFAULT)); motorStatus(MOTOR_SPEED_DEFAULT); }
int main (void) { SysCtlClockSet (SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_25MHZ); configureUART (); printf ("cpu-gauge.\r\n"); // Enable the GPIO port that is used for the on-board LED. SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3); // // USB config : Enable the GPIO peripheral used for USB, and configure the USB pins. // SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); // SysCtlGPIOAHBEnable(SYSCTL_PERIPH_GPIOD); // GPIOPinTypeUSBAnalog(GPIO_PORTD_AHB_BASE, GPIO_PIN_4 | GPIO_PIN_5); // ButtonsInit(); // printf ("Buttons OK.\r\n"); motorInit (); printf ("Motor initialized.\r\n"); printf ("Test sequence : +40 steps.\r\n"); motorRun (40); printf ("Test sequence : -40 steps.\r\n"); motorRun (-40); printf ("Test sequence : OK.\r\n"); SysCtlDelay (DELAY_COEFFICIENT_SCD / 10); uint8_t ui8ButtonsChanged, ui8Buttons; while (1) { ButtonsPoll(&ui8ButtonsChanged, &ui8Buttons); if (ui8ButtonsChanged) { if(ui8Buttons & LEFT_BUTTON) { printf ("pressed\r\n"); } else { printf ("released\r\n"); } } } }
int main(void) { /* * Initialize the System Timer, UART, TWI, SPI, * ADC and the UART menu task for user or software * interaction. Also enables interrupts! * Also, the UART will be tied to stdin, stdout and stderr. * This allows you to use stdio.h utilities like printf() */ xyInit(); printf("Initializing Hardware Test...\n"); /* * Initialize Hardware */ xyLed(LED_GREEN, LED_OFF); xyLed(LED_RED, LED_ON); motorInit(); orientationInit(); /* * Register Tasks in the Scheduler. A UART task * is already registered... */ addTask(&ledTask); // Blink LED /* * Add commands for the UART menu */ addMenuCommand('b', bluetoothString, &bluetoothTest); addMenuCommand('r', sensorString, &printRaw); addMenuCommand('t', ramString, &ramTest); addMenuCommand('v', voltageString, &printVoltage); printf("Hardware Test Initialized!\n"); /* * Execute all registered tasks, forever. */ for(;;) { tasks(); } return 0; }
void main(void) { motorInit(); stop(); __delay_cycles(32000000); while(1) { moveRight(); __delay_cycles(7500000); stop(); __delay_cycles(32000000); moveLeft(); __delay_cycles(7500000); stop(); __delay_cycles(32000000); } }
/** * \brief Initializes all the subsystems for this Droplet. This function MUST be called * by the user before using any other functions in the API. */ static void initAllSystems(void){ cli(); Config32MHzClock(); calculateIdNumber(); schedulerInit(); INIT_DEBUG_PRINT("SCHEDULER INIT\r\n"); pcCommInit(); INIT_DEBUG_PRINT("PC COM INIT\r\n"); rgbLEDinit(); INIT_DEBUG_PRINT("LED INIT\r\n"); powerInit(); INIT_DEBUG_PRINT("POWER INIT\r\n"); i2cInit(); INIT_DEBUG_PRINT("I2C INIT\r\n"); enableInterrupts(); rangeAlgsInit(); INIT_DEBUG_PRINT("RANGE ALGORITHMS INIT\r\n"); rgbSensorInit(); INIT_DEBUG_PRINT("RGB SENSE INIT\r\n"); irLedInit(); INIT_DEBUG_PRINT("IR LED INIT\r\n"); irSensorInit(); INIT_DEBUG_PRINT("IR SENSE INIT\r\n"); #ifdef AUDIO_DROPLET speakerInit(); INIT_DEBUG_PRINT("SPEAKER INIT\r\n"); micInit(); INIT_DEBUG_PRINT("MIC INIT\r\n"); //Must occur after ir_sensor_init. #endif motorInit(); INIT_DEBUG_PRINT("MOTOR INIT\r\n"); randomInit(); INIT_DEBUG_PRINT("RAND INIT\r\n"); //This uses adc readings for a random seed, and so requires that the adcs have been initialized. localizationInit(); INIT_DEBUG_PRINT("LOCALIZATION INIT\r\n"); #ifdef SYNCHRONIZED fireflySyncInit(); #endif setAllirPowers(256); startupLightSequence(); irCommInit(); INIT_DEBUG_PRINT("IR COM INIT\r\n"); #ifdef AUDIO_DROPLET enableMicInterrupt(); #endif }
task main() { clearDebugStream(); writeDebugStreamLine("This is PIDTest\n"); memset(&desiredEncVals, 0, sizeof(desiredEncVals)); motorInit(&desiredEncVals); semaphoreInitialize(PIDconstantSemaphore); startTask(PID); long loopStartTimeMs = nPgmTime; semaphoreLock(PIDconstantSemaphore); for (pid_kp=0; pid_kp<5; pid_kp++) { for (pid_ki=0; pid_ki<0; pid_ki+=0.01) { for (pid_kd=0; pid_kd<0; pid_kd++) { writeDebugStream("%f, %f, %f, ", pid_kp, pid_ki, pid_kd); if (bDoesTaskOwnSemaphore(PIDconstantSemaphore)) { semaphoreUnlock(PIDconstantSemaphore); } while(motorGetEncoder((tMotor) MecMotor_FR) < 5000){ hogCPU(); motorUpdateState(); desiredMotorVals.power[MecMotor_FR] = 50; releaseCPU(); } semaphoreLock(PIDconstantSemaphore); motor[MecMotor_FR] = 0; //can do this because we have lock on semaphore writeDebugStream("Changing constants!\n"); wait1Msec(1000); //wait for motor to spin down } } } }
void motorTest() { motorInit(); // Start PWM process. Period 1 ms, Freq 1 kHz pwm_start(basepwm); // Turn motors counterclockwise for 3 s. high(M1forward); high(M2forward); pwm_set(M1enable, 0, 1000); pwm_set(M2enable, 1, 1000); pause(2000); // Stop again pwm_set(M1enable, 0, 0); pwm_set(M2enable, 1, 0); // End the PWM process pwm_stop(); }
/*inicializa todos os sensores da mesa xy*/ void xyPlotterInit(){ uint8_t customChar[] = { 0x03, 0x06, 0x0C, 0x1F, 0x1F, 0x0C, 0x06, 0x03 }; SystemInit(); TM_HD44780_Init(16, 2); TM_HD44780_CreateChar(0, &customChar[0]); TM_HD44780_Puts(0, 0, "Init sensores..."); motorInit(); canetaInit(); endInit(); joystickInit(); buttonsInit(); Delayms(250); }
void moveMotors(int left, int right) { motorInit(); // Start PWM process. Period 1 ms, Freq 1 kHz pwm_start(basepwm); if(left >=0){ high(M1forward); } else { high(M1reverse); left=abs(left); } if(right >=0){ high(M2forward); } else { high(M2reverse); right=abs(right); } pwm_set(M1enable, 0, left); pwm_set(M2enable, 1, right); }
int main(void) { // Initialize all hardware PLL_Init(); eStopInit(); encoderInit(actlPos); motorInit(); lightsInit(); lightsUpdate(COLOR_RED); UART_Init(); Timer1_Init(); softRun(); // Send welcome message to UART terminal UART_OutChar('W');UART_OutChar('e');UART_OutChar('l');UART_OutChar('c'); UART_OutChar('o');UART_OutChar('m');UART_OutChar('e'); UART_OutChar(CR);UART_OutChar(LF); // Spin forever while(1) { parse(UART_InUDec()); // read commands from UART // All other functions performed by Timer 1 interrupt handler } }
int main(void) { motorInit(); // Initialize the motor port to drive the MOSFET low uint8_t watchDogResetFlag = MCUSR & _BV(WDRF); /// Save the watchdog flag MCUSR &= ~ _BV( WDRF ); // Clear the watchdog flag // "In safety level 1, WDE is overridden by WDRF in MCUSR...." // "This means that WDE is always set when WDRF is set." // IF we left this Set, then we could get watchdogged while sleeping wdt_enable( WDTO_8S ); // Give ourselves 8 seconds before forced reboot enableTimer0(); // Initialize the timer that also PWMs the LEDs WHITE_LED_DDR |= _BV(WHITE_LED_BIT); // Pin to output RED_LED_DDR |= _BV(RED_LED_BIT); // Button sense pin setup BUTTON_PORT |= _BV(BUTTON_BIT); // Enable pull-up for button pin // Battery Charger status pin setup EOC_PORT |= _BV(EOC_BIT); // Activate pull-up CIP_PORT |= _BV( CIP_BIT); // Activate pull-up _delay_us(1); // Give the pull-ups a second to work if ( !watchDogResetFlag ) { // Are we coming out of anything except for a WatchDog reset? // Cold boot, run test mode // Blink back and forth to show LEDs work and solicit a button press for(uint8_t i=0;i<100 && !BUTTON_STATE_DOWN(); i++ ) { setRedLED(255); for(uint8_t j=0; j<100 && !BUTTON_STATE_DOWN();j++ ) { _delay_ms(1); } setRedLED(0); setWhiteLED(255); for(uint8_t j=0; j<100 && !BUTTON_STATE_DOWN();j++ ) { _delay_ms(1); } setWhiteLED(0); wdt_reset(); } _delay_ms(BUTTON_DEBOUNCE_TIME_MS); // TODO: Put more code here for some testing and feedback on initial battery connection at the factory. } // Ready to begin normal operation! if (BUTTON_STATE_DOWN()) { // Possible stuck button? // If we get here, either we just finished test mode and the button is Still down, in which case // we are testing to make sure it goes back up // Otherwise we just reset and the button was down when we woke, so likely it is stuck down // and that is what caused the reset. In this case, show the user and then eventually disable the button. // Each pass of this loop takes ~1 sec. for( uint16_t t=0; (t <= BUTTON_TRANSIT_TIMEOUT_S) && BUTTON_STATE_DOWN(); t++ ) { // To indicate that we are in a stuck-button sequence, we will blink the white LED // at 50% brightness, 1Hz, 10% duty cycle. This looks different than other states // and also minimizes battery usage (the LED pulls 10+mA) since we might be doing this // for many minutes // Start with LED off because it looks better when we are coming in from a watchdog // reset because the button was held down for more than 8 secs. Otherwise user // sees an odd blink pattern. setWhiteLED(0); // Leave the white LED off for 900 ms or until the button goes up for( uint8_t l=0; l<90 && BUTTON_STATE_DOWN() ; l++) { _delay_ms(10); } setWhiteLED(BUTTON_FEEDBACK_BRIGHTNESS); // Leave the white LED on for 100 ms or until the button goes up // Could do this as a single _delay_ms(100) but that might feel un-responsive for( uint8_t l=0; l<10 && BUTTON_STATE_DOWN() ; l++) { _delay_ms(10); } wdt_reset(); } // Turn off LED before continuing setWhiteLED(0); // Debounce the possible button up transition _delay_ms(BUTTON_DEBOUNCE_TIME_MS); } if (BUTTON_STATE_DOWN()) { // Do we still have a stuck button? // Indicate we are entering transit mode with a quick double flash of both LEDs setRedLED(255); setWhiteLED(255); _delay_ms(100); setRedLED(0); setWhiteLED(0); _delay_ms(100); setRedLED(255); setWhiteLED(255); _delay_ms(100); setRedLED(0); setWhiteLED(0); BUTTON_PORT &= ~_BV(BUTTON_BIT); // Disable pull up to avoid running the battery down. // Do not enable interrupt on button pin change - we will require a charger state change to wake up // Since the interrupt is not enabled, the pin will be disconnected during sleep so any floating // on it will not waste power. } else { // Leave pull-up enabled PCMSK1 = _BV(BUTTON_INT); // Enable interrupt on button pin so we wake on a press } PCMSK0 = _BV(EOC_INT) | _BV(CIP_INT); // Enable interrupt on change in state-of-charge pin or end-of-charge pin no matter what GIMSK |= _BV(PCIE1) | _BV(PCIE0); // Enable both pin change interrupt vectors (each individual pin was also be enabled above) // Clear pending interrupt flags. This way we will only get an interrupt if something changes // after we read it. There is a race condition where something could change between the flag clear and the // reads below, so code should be able to deal with possible redundant interrupt and worst case // is that we get woken up an extra time and go back to sleep. GIFR = _BV(PCIF1) | _BV(PCIF0); // Clear interrupt flags so we will interrupt on any change after now... if ( !CIP_STATE_ACTIVE() && !EOC_STATE_ACTIVE() ) { // Check if conditions are ALREADY true since we only wake on change.... // Ok, it is bedtime! set_sleep_mode( SLEEP_MODE_PWR_DOWN ); // Go into deep sleep where only a pin change can wake us.. uses only ~0.1uA! // GOOD NIGHT! // This code disables the Watchdog. Note that we can not use the library wdt_disable() becuase it has a bug // that causes intermittent unwanted resets. // Note interrupts are already clear when we get here, otherwise we would need to worry about getting interrupted between the two following lines WDTCSR |= _BV(WDCE) | _BV(WDE); // In the same operation, write a logic one to WDCE and WDE. // Note we use OR to preserve the prescaler WDTCSR = 0; // Within the next four clock cycles, in the same operation, write the WDE and WDP bits // as desired, but with the WDCE bit cleared. sleep_enable(); // "To enter any of the three sleep modes, the SE bit in MCUCR must be written to logic one and a SLEEP instruction must be executed." sei(); // Enable global interrupts. "When using the SEI instruction to enable interrupts, the instruction following SEI will be executed before any pending interrupts." sleep_cpu(); // This must come right after the sei() to avoid race condition // GOOD MORNING! // If we get here, then a button push or change in charger status woke s up.... sleep_disable(); // "To avoid the MCU entering the sleep mode unless it is the programmer’s purpose, it is recommended to write the Sleep Enable (SE) bit to one just before the execution of the SLEEP instruction and to clear it immediately after waking up." cli(); // We are awake now, and do don't care about interrupts anymore (out interrupt routines don't do anything anyway) wdt_enable( WDTO_8S ); // Re-enable watchdog on wake Give ourselves 8 seconds before reboot } // Ok, now we are running!!! // Motor speed uint8_t currentSpeedStep = 0; // What motor speed setting are we currently on? while (1) { // This main loop runs for as long as the motor is on. // It can be terminated by battery charger change of state, low battery detection, button press back to 0 speed, or long button press // All these changes terminate the loop in a reboot. if (EOC_STATE_ACTIVE()) { // End of charge? motorOff(); //Turn motor off in case were running before plug went in setWhiteLED(255); // White LED full on _delay_ms( JACK_DEBOUNCE_TIME_MS ); while (EOC_STATE_ACTIVE()); // White LED on for as long as we are charging.... // Note that this will watchdog timeout after 8 seconds and reboot us, // After which we will immediately fall right back to here and continue to show the white LED setWhiteLED(0); // Turn it off now, for instant feedback if unplugged (otherwise it will be on for extra 250ms waiting for watchdog reset) // Charger unplugged, reboot for goo measure REBOOT(); } if (CIP_STATE_ACTIVE()) { // Charging? motorOff(); //Turn motor off in case were running before plug went in uint8_t brightness=0; int8_t direction=1; _delay_ms( JACK_DEBOUNCE_TIME_MS ); while (CIP_STATE_ACTIVE()) { // White LED pulse for as long as we are charging.... setWhiteLED(brightness); if (brightness==255) { direction=-1; } else if (brightness==0) { direction=1; } brightness+=direction; _delay_ms(1); // Slows the speed of the rampping LED wdt_reset(); } setWhiteLED(0); // Turn it off now, for instant feedback if unplugged (otherwise it will be on for extra 250ms waiting for watchdog reset) // All done charing, reboot for good measure REBOOT(); } uint8_t vccx10 = readVccVoltage(); // Capture the current power supply voltage. This takes ~1ms and will be needed multiple times below if ( vccx10 <= LOW_BATTERY_VOLTS_COLDx10) { if ( (currentSpeedStep==0) || ( vccx10 <= LOW_BATTERY_VOLTS_WARMx10) ) { // Motor off, or running and really low? motorOff(); setWhiteLED(0); // Needed becuase both LEDs might be on if we are in the middle of a button press setRedLED(255); _delay_ms(LOW_BATTERY_LED_ONTIME_MS); // Show red LED to user to show low battery while (BUTTON_STATE_DOWN()); // Wait for button to be released if pressed // Will watchdog timeout in 8 seconds if stuff setRedLED(0); REBOOT(); } } uint8_t buttonPressedFlag=0; if (BUTTON_STATE_DOWN()) { // Button pushed? setWhiteLED(BUTTON_FEEDBACK_BRIGHTNESS); _delay_ms(BUTTON_DEBOUNCE_TIME_MS); // debounce going down... if ( currentSpeedStep ==0 ) { // Special case first press turning on instantly updateMotor( pgm_read_word(&speedSteps[1].top) , pgm_read_word(&speedSteps[1].normailzedDuty), vccx10); // Set new motor speed } uint16_t buttonDownCount=0; while (BUTTON_STATE_DOWN()) { // Wait for button to go back up or longpress timeout if (buttonDownCount++ >= BUTTON_LONG_PRESS_MS ) { // Long press? Shut motor off // The reboot would do both of these anyway, but we do them redundantly here so UI feels responsive- // The full reboot cycle takes 100+ ms. motorOff(); setWhiteLED(0); REBOOT(); // If the button is still down once we reboot, we will land in the stuck button detection sequence // which will blink the LED for 1/10th second every second until either the button goes up // or the transit mode button timeout expires } _delay_ms(1); // One loop=~1ms } // Pressed less than a long press buttonPressedFlag=1; // Debounce after setting new motor speed so UI feels responsive currentSpeedStep++; if (currentSpeedStep >= SPEED_STEP_COUNT ) { // Cycled all the Way around? currentSpeedStep=0; } } updateMotor( pgm_read_word(&speedSteps[currentSpeedStep].top) , pgm_read_word(&speedSteps[currentSpeedStep].normailzedDuty), vccx10); // Set new motor speed if (buttonPressedFlag) { // Button released, white LED off again setWhiteLED(0); _delay_ms(BUTTON_DEBOUNCE_TIME_MS); // debounce the button returning back up } if (currentSpeedStep==0) { // Either we stepped though the settings back to off, or we got a spurious wake up REBOOT(); } // If we get to here, then we check for a low battery and had the chance to reboot if we found one, // so ok to postpone reset... wdt_reset(); } }
void init(void) { #ifdef USE_HAL_DRIVER HAL_Init(); #endif printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; systemInit(); //i2cSetOverclock(masterConfig.i2c_overclock); // initialize IO (needed for all IO operations) IOInitGlobal(); debugMode = masterConfig.debug_mode; #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); #ifdef ALIENFLIGHTF3 ledInit(hardwareRevision == AFF3_REV_1 ? false : true); #else ledInit(false); #endif LED2_ON; #ifdef USE_EXTI EXTIInit(); #endif #if defined(BUTTONS) gpio_config_t buttonAGpioConfig = { BUTTON_A_PIN, Mode_IPU, Speed_2MHz }; gpioInit(BUTTON_A_PORT, &buttonAGpioConfig); gpio_config_t buttonBGpioConfig = { BUTTON_B_PIN, Mode_IPU, Speed_2MHz }; gpioInit(BUTTON_B_PORT, &buttonBGpioConfig); // Check status of bind plug and exit if not active delayMicroseconds(10); // allow GPIO configuration to settle if (!isMPUSoftReset()) { uint8_t secondsRemaining = 5; bool bothButtonsHeld; do { bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN); if (bothButtonsHeld) { if (--secondsRemaining == 0) { resetEEPROM(); systemReset(); } delay(1000); LED0_TOGGLE; } } while (bothButtonsHeld); } #endif #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated #if !defined(USE_HAL_DRIVER) dmaInit(); #endif #if defined(AVOID_UART1_FOR_PWM_PPM) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); #elif defined(AVOID_UART2_FOR_PWM_PPM) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); #elif defined(AVOID_UART3_FOR_PWM_PPM) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); #else serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #ifdef USE_SERVOS servoMixerInit(masterConfig.customServoMixer); #endif uint16_t idlePulse = masterConfig.motorConfig.mincommand; if (feature(FEATURE_3D)) { idlePulse = masterConfig.flight3DConfig.neutral3d; } if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) { featureClear(FEATURE_3D); idlePulse = 0; // brushed motors } #ifdef USE_QUAD_MIXER_ONLY motorInit(&masterConfig.motorConfig, idlePulse, QUAD_MOTOR_COUNT); #else motorInit(&masterConfig.motorConfig, idlePulse, mixers[masterConfig.mixerMode].motorCount); #endif #ifdef USE_SERVOS if (isMixerUsingServos()) { //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); servoInit(&masterConfig.servoConfig); } #endif #ifndef SKIP_RX_PWM_PPM if (feature(FEATURE_RX_PPM)) { ppmRxInit(&masterConfig.ppmConfig, masterConfig.motorConfig.motorPwmProtocol); } else if (feature(FEATURE_RX_PARALLEL_PWM)) { pwmRxInit(&masterConfig.pwmConfig); } pwmRxSetInputFilteringMode(masterConfig.inputFilteringMode); #endif mixerConfigureOutput(); #ifdef USE_SERVOS servoConfigureOutput(); #endif systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER beeperInit(&masterConfig.beeperConfig); #endif /* temp until PGs are implemented. */ #ifdef INVERTER initInverter(); #endif #ifdef USE_BST bstInit(BST_DEVICE); #endif #ifdef USE_SPI #ifdef USE_SPI_DEVICE_1 spiInit(SPIDEV_1); #endif #ifdef USE_SPI_DEVICE_2 spiInit(SPIDEV_2); #endif #ifdef USE_SPI_DEVICE_3 #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_2) { spiInit(SPIDEV_3); } #else spiInit(SPIDEV_3); #endif #endif #ifdef USE_SPI_DEVICE_4 spiInit(SPIDEV_4); #endif #endif #ifdef VTX vtxInit(); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_USART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI) #if defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE); #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableVBat = feature(FEATURE_VBAT); adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif #ifdef USE_RTC6705 if (feature(FEATURE_VTX)) { rtc6705_soft_spi_init(); current_vtx_channel = masterConfig.vtx_channel; rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power); } #endif #ifdef OSD if (feature(FEATURE_OSD)) { osdInit(); } #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, masterConfig.mag_declination, masterConfig.gyro_lpf, masterConfig.gyro_sync_denom)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; LED2_OFF; for (int i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif imuInit(); mspFcInit(); mspSerialInit(); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( &masterConfig.gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { sonarInit(&masterConfig.sonarConfig); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USB_CABLE_DETECTION usbCableDetectInit(); #endif #ifdef TRANSPONDER if (feature(FEATURE_TRANSPONDER)) { transponderInit(masterConfig.transponderData); transponderEnable(); transponderStartRepeating(); systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(IO_TAG_NONE); } #elif defined(USE_FLASH_M25P16) m25p16_init(IO_TAG_NONE); #endif flashfsInit(); #endif #ifdef USE_SDCARD bool sdcardUseDMA = false; sdcardInsertionDetectInit(); #ifdef SDCARD_DMA_CHANNEL_TX #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip #if defined(STM32F4) || defined(STM32F7) sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM; #else sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; #endif #else sdcardUseDMA = true; #endif #endif sdcard_init(sdcardUseDMA); afatfs_init(); #endif if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) { masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed masterConfig.gyro_sync_denom = 1; } setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime #ifdef BLACKBOX initBlackbox(); #endif if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayResetPageCycling(); displayEnablePageCycling(); #endif } #endif #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; fcTasksInit(); systemState |= SYSTEM_STATE_READY; }
/* * Application entry point. */ int main(void) { int8_t accelData[2]={0,0}; // Discovery Board's Accelerometer uint8_t receivedBuff[4]={0,0,0,0}; // Received request/information from PandaBoard uint8_t sentData[4] = {0,0,0,0}; // Returned Information (reply) to the PandaBoard float imuData[7]={0,0,0,0,0,0,0}; // IMU calculated data based on Razor Boad int* razorInfo; // Razor Board Data int steering = 0; int speed = 0; int ir_data[3]={0,0,0}; int16_t us_data[3]={0,0,0}; /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ halInit(); chSysInit(); mypwmInit(); // Initializing Motor motorInit(); // Initializing IR Thread ADCinit(); // Initializing US Thread // myUltrasonicInit(); // Initializing Discovery Board's Accelerometer //mySPIinit(); // Initializing Razor Board myRazorInit(); // Activates the USB driver and then the USB bus pull-up on D+. myUSBinit(); // Initializing IMU Calculations. initIMU(); //Starting the usb configuration sdStart(&SDU1,&portConfig2); char receivedInfo[11]; /* * Main loop, it takes care of reciving the requests from Panda Board using USB protocol, * and reply with the requested data. */ while (TRUE) { receivedInfo[0]='T'; sdRead(&SDU1, receivedInfo, 10); // getImuValues(imuData); // getAccel(accelData); // getIR(ir_data); // getUS(us_data); if(receivedInfo[0] != 'T'){ receivedInfo[11]='\0'; parse(receivedInfo); //setMotorData(-(rcvData[1]-28),rcvData[2]-2); setMotorData(rcvData[1],1550); translate(rcvData[0],ir_data,us_data,razorInfo,imuData,accelData,sentData); sdWrite(&SDU1, sentData, 4); } } }