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);
}
}
}
