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