int main(void) { char ch; LockoutProtection(); InitializeMCU(); initUART(); while(1) { UARTprintf("\nROBZ DEMO\n"); UARTprintf(" 0=UART Demo\n 1=Motor Demo\n"); UARTprintf(" 2=Servo Demo\n 3=Line Sensor\n"); UARTprintf(" 4=IR Sensor Demo\n 5=Encoders Demo\n"); UARTprintf(">> "); ch = getc(); putc(ch); UARTprintf("\n"); if (ch == '0') { UARTprintf("\nUART Demo\n"); uartDemo(); } else if (ch == '1') { UARTprintf("\nMotor Demo\n"); initMotors(); motorDemo(); } else if (ch == '2') { UARTprintf("\nServo Demo\n"); initServo(); servoDemo(); } else if (ch == '3') { UARTprintf("\nLine Sensor Demo\n"); initLineSensor(); lineSensorDemo(); } else if (ch == '4') { UARTprintf("\nIR Sensor Demo\n"); initIRSensor(); IRSensorDemo(); } else if (ch == '5') { UARTprintf("\nEncoders Demo\n"); initEncoders(); encoderDemo(); } } }
Mobile::Mobile( const vec2 & v, ftype angle) :Oriented( v, angle ), speed( 0, 0), rotationSpeed(0), foodEatingTicker( 0.2 ) { mass = BOT_MASS; inertion = BOT_INERTION; movementFriction = MOVEMENT_FRICTION; rotationFriction = ROTATION_FRICTION; energy = 1; initMotors(); world = NULL; foodEaten = 0; birthday = -1; }
int main () { initMotors(); open(5); unsigned char rx_str[50]; while (1) { read(rx_str,10,ON); if(!(strncmp(rx_str,"fwd",3))) {fwd(rx_str[4]);} if(!(strncmp(rx_str,"bwd",3))) {bwd(rx_str[4]);} if(!(strncmp(rx_str,"lft",3))) {lft(rx_str[4]);} if(!(strncmp(rx_str,"rgh",3))) {rgh(rx_str[4]);} if(!(strncmp(rx_str,"slft",4))) {slft(rx_str[4]);} if(!(strncmp(rx_str,"srgh",4))) {srgh(rx_str[4]);} if(!(strncmp(rx_str,"blft",4))) {blft(rx_str[4]);} if(!(strncmp(rx_str,"brgh",4))) {brgh(rx_str[4]);} if(!(strncmp(rx_str,"stop",4))) {stop();} } return 0; }
int main() { signal(SIGINT, &cleanup); wiringPiSetup(); jandroid.motors = initMotors(); jandroid.servos = initServos(); jandroid.socket = init_socket(); while((jandroid.client_socket = add_client(jandroid.socket)) > 0) { printf("%s\n", "Listening for command"); listen_command(jandroid.motors, jandroid.servos, jandroid.client_socket); } cleanup(0); return 0; }
/* * Application entry point. */ int main(void) { /* * 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(); /* * Activates the serial driver 2 using the driver default configuration. * PA2(TX) and PA3(RX) are routed to USART2. */ sdStart(&SD2, NULL); palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7)); palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7)); /* * Initialize Motors */ initMotors(); /* * Creates the example thread. */ chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL); /* * Normal main() thread activity, in this demo it just performs * a shell respawn upon its termination. */ while (TRUE) { if (palReadPad(GPIOA, GPIOA_BUTTON)) TestThread(&SD2); chThdSleepMilliseconds(500); } return 5;//lol }
int main(void) { BOARD_Init(); SERIAL_Init(); //Init the motors, duh initMotors(); char input; dbprintf("Welcome to the Motor Test Harness\n"); dbprintf("Speed: 0-10, f: forward, s: backward\n"); dbprintf("r: right motor, l: left motor, b: battery\n"); while (1) { input = GetChar(); char_input(input); DELAY(A_BIT) } return 0; }
/* * main.c */ int main(void) { WDTCTL = WDTPW|WDTHOLD; // stop the watchdog timer initMotors(); while(1) { TurnLeft(); ShortDelay(); StopBot(); Delay(); TurnRight(); ShortDelay(); StopBot(); Delay(); MoveForward(); Delay(); StopBot(); Delay(); MoveBack(); Delay(); StopBot(); Delay(); TurnRight(); Delay(); StopBot(); Delay(); TurnLeft(); Delay(); StopBot(); Delay(); Delay(); Delay(); } }
//---------------------------------------------------------------------- //---------------------------------------------------------------------- int main(void) { P1DIR |= BIT0; // Set the red (left) LED as output P1DIR |= BIT6; // Set the green (right) LED as output IFG1=0; // clear interrupt flag1 WDTCTL = WDTPW + WDTHOLD; // disable WDT BCSCTL1 = CALBC1_8MHZ; // 8MHz clock DCOCTL = CALDCO_8MHZ; initMotors(); stop(); bFunctionality(); } //End Main
int main(void) { int pwm; int adc_on; int adc_off; uint16_t i; element elem; robot.rotation_target = 0; robot.right_motor_pos =0; robot.left_motor_pos =0; robot.right_motor_target =0; robot.left_motor_target =0; SysTick_Config(168000000/8/1000000); // interrupr 1000000 per secound initTimer3(1); initMotors(); initEncoders(); initUsart3(); // enableUSART3RXNEInterrupt(); initADC(); initIRSensors(); initLED(); initSPI2(); LED_OFF; waitMs(1000); LED_ON; initPIDStructures(); stopMotors(); resetEncoders(); updateRobotState(); robot.left_ir_sensor_target = measures.left_ir_sensor; robot.right_ir_sensor_target = measures.right_ir_sensor; initQueue(&robot_queue); //addMove(&robot_queue,FORWARD,2000); addMove(&robot_queue,ROTATE,20000); // addMove(&robot_queue,FORWARD,9000); // addMove(&robot_queue,ROTATE,0); // addMove(&robot_queue,ROTATE,6000); // addMove(&robot_queue,FORWARD,9000); // addMove(&robot_queue,ROTATE,6000); // addMove(&robot_queue,ROTATE,12000); // addMove(&robot_queue,FORWARD,9000); // addMove(&robot_queue,ROTATE,12000); // addMove(&robot_queue,ROTATE,3000); // nextMove(); LED_ON; while(1) { if ( isBatteryWeak() ){ stopMotors(); blinkLed(); } if (flags.update_robot == 1){ moveRobot(); updateRobotState(); flags.update_robot =0; } if (flags.usart_custom == 1){ USART3_transmitString(itoa((int) robot.rotation, buf,10)); USART3_transmitString(" "); USART3_transmitString(itoa((int) robot.rotation_target, buf,10)); USART3_transmitString("\n"); } } // flags.usart_custom == 0; // USART3_transmitString("lewe_kolo_diff "); // USART3_transmitString(itoa((int) robot.left_motor_target - robot.left_motor_pos , buf,10)); // USART3_transmitByte('\n'); // USART3_transmitString("lewe kolo pwm "); // USART3_transmitString(itoa((int) getTranslation(LEFT_MOTOR) , buf,10)); // USART3_transmitByte('\n'); // // USART3_transmitString("prawe kolo diff "); // USART3_transmitString(itoa((int) robot.right_motor_target - robot.right_motor_pos , buf,10)); // USART3_transmitByte('\n'); // USART3_transmitString("prawe kolo pwm "); // USART3_transmitString(itoa((int) getTranslation(RIGHT_MOTOR) , buf,10)); // USART3_transmitByte('\n'); // } // // if (flags.usart_graph == 1){ // static uint8_t usart_start = 1; // if (usart_start ){ // usart_start=0; // USART3_transmitString("\n"); // USART3_transmitString("\n"); // USART3_transmitString("\n"); // USART3_transmitString("__start__\n"); // USART3_transmitString("lewe_kolo_pozycja "); // USART3_transmitString("lewe_kolo_cel "); // USART3_transmitString("lewe_kolo_diff "); // USART3_transmitString("lewe_kolo_pwm_T "); // USART3_transmitString("prawe_kolo_pozycja "); // USART3_transmitString("prawe_kolo_cel "); // USART3_transmitString("prawe_kolo_diff "); // USART3_transmitString("prawe_kolo_pwm_T \n"); // USART3_transmitString(" \n"); // } // // USART3_transmitString(itoa((int) robot.right_motor_pos , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) robot.right_motor_target , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) robot.right_motor_target - robot.right_motor_pos , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) getTranslation(RIGHT_MOTOR) , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) robot.left_motor_pos , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) robot.left_motor_target , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) robot.left_motor_target - robot.left_motor_pos , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) getTranslation(LEFT_MOTOR) , buf,10)); // USART3_transmitByte('\n'); // flags.usart_graph = 0; // } // } // return 0; }
//*MAIN*// int main(int argc, char * argv[]) { bool active = true; //flag that controls main loop int value; //value of received message char messageType; unsigned int serialPort; char * serialAddr = "/dev/ser1"; TCP tcpConnection; unsigned int clientSocket; //Initialization //Parse the arguments switch (argc) { case 2: serialAddr = argv[1]; break; } if ( argc >= 2 ) { if ( initSerial(serialPort, serialAddr) == false ) { std::cout << "Serial port initialization failure.\n"; return 0; } if ( initMotors() == false ) { std::cout << "Motor initialization failure.\n"; return 0; } } if ( initTCP(tcpConnection, clientSocket) == false ) { std::cout << "TCPIP initialization failure.\n"; return 0; } while (active) { tcpConnection.receiveData( clientSocket, (char *) &messageType, sizeof( messageType )); if(tcpConnection.receiveData( clientSocket, (char *) &value, sizeof( value )) == DATA_ERROR) { active = false; std::cout << "Client Disconnected!" << std::endl; } #ifdef DEBUG debugprint(messageType, value); #endif if(messageType == 'X') active = false; else if (active == true) { #ifdef ENABLE_STEERING if (messageType == 'S') Move_Motor_Abs(value); else #endif motorControl(serialPort, messageType, value); //watch out for this! } } std::cout << "Resetting motors..." << std::endl; resetMotors(serialPort); #ifdef ENABLE_STEERING Close_Maxon_Motor_Driver(); #endif tcpConnection.closeSocket(clientSocket); std::cout << "Terminating..." << std::endl; return 0; }
int main(void) { #ifdef NEED_EVENT uint32_t DVSEventPointer; uint32_t DVSEventTimeLow; uint16_t DVSEvent; uint32_t timeStampMemory = 0, timeStampDelta = 0; #endif int16_t angle = 0; uint32_t cnt =0; ExtraPinsInit(); disablePeripherals(); Chip_RIT_Init(LPC_RITIMER); RTC_TIME_T build = { .time = { BUILD_SEC_INT, BUILD_MIN_INT, BUILD_HOUR_INT, BUILD_DAY_INT, 0, 1, BUILD_MONTH_INT, BUILD_YEAR_INT } }; buildTime = build; //This should be one of the first initializations routines to run. sensorsInit(); #ifdef NEED_EVENT DVS128ChipInit(); #endif DacInit(); UARTInit(LPC_UART, BAUD_RATE_DEFAULT); /* baud rate setting */ initMotors(); PWMInit(); #if USE_IMU_DATA timerDelayMs(100); MPU9105Init(); #endif #if USE_SDCARD SDCardInit(); #endif #if USE_PUSHBOT MiniRobInit(); #endif #ifdef TEST_RUN test(); //This will not return #endif LED1SetOn(); // Start M0APP slave processor cr_start_m0(&__core_m0app_START__); LED1SetOff(); LED0SetOn(); LED0SetBlinking(ENABLE); UARTShowVersion(); for (;;) { if (ledBlinking && toggleLed0) { LED0Toggle(); toggleLed0 = 0; } // ***************************************************************************** // UARTIterate(); // ***************************************************************************** while (bytesReceived(&uart)) { // incoming char available? UART0ParseNewChar(popByteFromReceptionBuffer(&uart)); } // ***************************************************************************** // Deal with audio data // ***************************************************************************** /* * do the fft cross correlation and figure out the angle * manipulate the proceeding direction of the pushbot */ // if buffer reaches a length of 1024 /* if(process_flag != -1){ // -1 for not ready, 0 for buffer0, 1 for buffer1 // SysTick->CTRL &= ~0x1; //xprintf("%d\n", process_flag); angle = itd(); //SysTick->CTRL |= 0x1; if(++cnt>150){ xprintf("angle = %d\n", angle); cnt = 0; } }*/ // start doing math #if USE_IMU_DATA updateIMUData(); #endif #if USE_PUSHBOT refreshMiniRobSensors(); if (motor0.updateRequired) { motor0.updateRequired = 0; updateMotorController(MOTOR0); } if (motor1.updateRequired) { motor1.updateRequired = 0; updateMotorController(MOTOR1); } #endif /* // disable the data streaming through serial if (sensorRefreshRequested) { sensorRefreshRequested = 0; for (int i = 0; i < sensorsEnabledCounter; ++i) { if (enabledSensors[i]->triggered) { enabledSensors[i]->refresh(); enabledSensors[i]->triggered = 0; } } } */ #ifdef NEED_EVENT // ***************************************************************************** // processEventsIterate(); // ***************************************************************************** if (events.eventBufferWritePointer == events.eventBufferReadPointer) { // more events in buffer to process? continue; } if (eDVSProcessingMode < EDVS_PROCESS_EVENTS) { //Not processing events if (freeSpaceForTranmission(&uart) < (TX_BUFFER_SIZE - 32) || !(eDVSProcessingMode & EDVS_STREAM_EVENTS)) { #if LOW_POWER_MODE uart.txSleepingFlag = 1; __WFE(); #endif continue; //Wait until the buffer is empty. } } /*We are either processing events or streaming them. * If streaming the buffer must be empty at this point */ events.ringBufferLock = true; events.eventBufferReadPointer = ((events.eventBufferReadPointer + 1) & DVS_EVENTBUFFER_MASK); // increase read pointer DVSEventPointer = events.eventBufferReadPointer; // cache the value to be faster DVSEvent = events.eventBufferA[DVSEventPointer]; // fetch event from buffer DVSEventTimeLow = events.eventBufferTimeLow[DVSEventPointer]; // fetch event from buffer events.ringBufferLock = false; if (eDVSProcessingMode & EDVS_STREAM_EVENTS) { if (freeSpaceForTranmission(&uart) > 6) { // wait for TX to finish sending! pushByteToTransmission(&uart, (DVSEvent >> 8) | 0x80); // 1st byte to send (Y-address) pushByteToTransmission(&uart, DVSEvent & 0xFF); // 2nd byte to send (X-address) if (eDVSDataFormat == EDVS_DATA_FORMAT_BIN_TSVB) { // Calculate delta... timeStampDelta = DVSEventTimeLow - timeStampMemory; timeStampMemory = DVSEventTimeLow; // Save the current TS in delta // check how many bytes we need to send if (timeStampDelta < 0x7F) { // Only 7 TS bits need to be sent pushByteToTransmission(&uart, (timeStampDelta & 0x7F) | 0x80); // 3rd byte to send (7bit Delta TS, MSBit set to 1) } else if (timeStampDelta < 0x3FFF) { // Only 14 TS bits need to be sent pushByteToTransmission(&uart, (timeStampDelta >> 7) & 0x7F); // 3rd byte to send (upper 7bit Delta TS, MSBit set to 0) pushByteToTransmission(&uart, (timeStampDelta & 0x7F) | 0x80); // 4th byte to send (lower 7bit Delta TS, MSBit set to 1) } else if (timeStampDelta < 0x1FFFFF) { // Only 21 TS bits need to be sent pushByteToTransmission(&uart, (timeStampDelta >> 14) & 0x7F); // 3rd byte to send (upper 7bit Delta TS, MSBit set to 0) pushByteToTransmission(&uart, (timeStampDelta >> 7) & 0x7F); // 4th byte to send (middle 7bit Delta TS, MSBit set to 0) pushByteToTransmission(&uart, (timeStampDelta & 0x7F) | 0x80); // 5th byte to send (lower 7bit Delta TS, MSBit set to 1) } else {
/* * main.c * Author: Ian R Goodbody * Function: Implements maze navigation */ void main(void) { WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer initRangeFinders(); initMotors(); stop(); P1DIR |= LEFT_LED|RIGHT_LED; P1OUT &= ~(LEFT_LED|RIGHT_LED); int fBuffer[BUFFER_LN]; // Code probably unnecessary but it cannot hurt int lBuffer[BUFFER_LN]; int rBuffer[BUFFER_LN]; int fMedian; int lMedian; int rMedian; unsigned int lWheelSpeed = 4040; unsigned int rWheelSpeed = 4000; unsigned int runTime = 350; int i; fillBuffers(fBuffer, lBuffer, rBuffer); while(1) { P1OUT &= ~(LEFT_LED|RIGHT_LED); // turn off LEDs stop(); for (i = BUFFER_LN; i > 0; i--) { readFront(fBuffer); waitMiliseconds(1); readLeft(lBuffer); waitMiliseconds(1); readRight(rBuffer); waitMiliseconds(1); } fMedian = median(fBuffer); lMedian = median(lBuffer); rMedian = median(rBuffer); if(fMedian < 0x01F0) // Crash into wall test; ~2.5 inch threshold { if(lMedian < 0x01FF) // There is no wall remotely close to the left side, { // Initiate sweeping turn lWheelSpeed = 2600; rWheelSpeed = 5000; } else if(lMedian < 0x0266) // Getting too far from the wall start turning in { P1OUT |= LEFT_LED; // Red LED on, green off P1OUT &= ~RIGHT_LED; rWheelSpeed = 4270; lWheelSpeed = 4040; } else if(lMedian > 0x02E4) // Getting too close to the wall start turning out { P1OUT |= RIGHT_LED; // Green LED on, red off P1OUT &= ~LEFT_LED; rWheelSpeed = 3900; lWheelSpeed = 4040; } else // Acceptable distance from wall, cruise forward normally { //P1OUT |= RIGHT_LED; P1OUT &= ~(RIGHT_LED|LEFT_LED); rWheelSpeed = 4000; lWheelSpeed = 4040; } setLeftWheel(lWheelSpeed, FORWARD); setRightWheel(rWheelSpeed, FORWARD); runTime = 350; } else { // About to run into a wall, initiate hard right turn P1OUT |= RIGHT_LED|LEFT_LED; setLeftWheel(5080,FORWARD); setRightWheel(5260, BACKWARD); runTime = 450; } go(); waitMiliseconds(runTime); } }