int main(void) { // Disable interrupts. ("Clear interrupt bit") cli(); // One-time setup operations. setupSerialPort(); setupRightLED(); setupLeftLED(); setupTimer(); // Enable interrupts. ("Set interrupt bit") sei(); byteTx(128); // Start the open interface. byteTx(132); // Switch to full mode. // Toggle the LEDs once each second. while(2+2==4) { rightLEDon(); leftLEDoff(); byteTx(139); // Opcode for "Set LEDs" byteTx(10); // Led bits: both on byteTx(0); // Power led color: Fully green byteTx(255); // Power led intensity delayMs(1000); rightLEDoff(); leftLEDon(); byteTx(139); // Opcode for "Set LEDs" byteTx(0); // Led bits: both off byteTx(255); // Power led color: Fully red byteTx(255); // Power led intensity delayMs(1000); } }
void initializeCommandModule(void){ // Disable interrupts. ("Clear interrupt bit") cli(); // One-time setup operations. setupIOPins(); setupTimer(); setupSerialPort(); // Enable interrupts. ("Set interrupt bit") sei(); }
int main() { // Disable interrupts. ("Clear interrupt bit") cli(); // One-time setup operations. setupSerialPort(); setupRightLED(); setupLeftLED(); setupTimer(); // Enable interrupts. ("Set interrupt bit") sei(); byteTx(128); // Start the open interface. byteTx(132); // Switch to full Mode while(2+2==4) { delayMs(100); // Ask the robot about its bump sensors. byteTx(142); // Opcode for "Read sensors" byteTx(7); // Sensor packet 7: Bumps and wheel drops // Read the one-byte response and extract the relevant bits. uint8_t bumps = byteRx(); uint8_t bumpRight = bumps & (1 << 0); uint8_t bumpLeft = bumps & (1 << 1); // Set the command module LEDs based on this sensor data. if(bumpLeft) { leftLEDon(); } else { leftLEDoff(); } if(bumpRight) { rightLEDon(); } else { rightLEDoff(); } } }
int main(int argc, char *argv[]) { /* Initialize variables */ int iteration = 0; // Control loop counter int timestep = 100000; // Timestep in microseconds struct timeval timeBegin; // Control loop start time struct timeval timeStart, timeEnd; // Timing related long computationTime = 0; // Timing related /* Load cliff thresholds, seed RNG */ loadCliffThresholds(); srand(0); /* ************************************************************************ * Handle command line arguments * ***********************************************************************/ if (argc < 2) { fprintf(stderr, "Portname argument required -- something like -p /dev/tty.usbserial\n"); return 0; } while (1) { static struct option long_options[] = { {"port", required_argument, 0, 'p'}, {"timestep", required_argument, 0, 't'}, {"help", no_argument, 0, 'h'}, {0, 0, 0, 0} }; int c; int option_index = 0; c = getopt_long(argc, argv, "p:t:",long_options, &option_index); /* Detect end of options */ if (c == -1) break; /* Set options based on command line arguments */ switch(c) { case 'p': portName = optarg; break; case 't': timestep = strtol(optarg, (char **) NULL, 10); break; case '?': fprintf(stderr,"ERROR: Unknown command line argument\n"); exit(EXIT_FAILURE); } } /* ************************************************************************ * Set up resources used by program * ***********************************************************************/ /* Initialize mutex locks */ pthread_mutex_init(&pktNumMutex, NULL); pthread_mutex_init(&serialMutex, NULL); pthread_mutex_init(&lastActionMutex, NULL); pthread_mutex_init(&endFlagMutex, NULL); pthread_mutex_init(&resetPhaseMutex,NULL); /* Install signal handler */ struct sigaction act; // The new sigaction struct sigaction oldact; // To preserve old sigaction act.sa_handler = endProgram; // Set endProgram() as sig handler sigemptyset(&act.sa_mask); // Empty act's signal set act.sa_flags = 0; // Set act's flags to 0 sigaction(SIGINT, &act, &oldact); // Set act to respond to SIGINT /* Set up serial port and begin receiving data */ if (portName != NULL) setupSerialPort(portName); usleep(20000); // wait for at least one packet to have arrived if (0 != pthread_create(&tid, NULL, (void *) &csp3, NULL)) { perror("Could not create thread"); exit(EXIT_FAILURE); } /* Get time just before control loop starts */ gettimeofday(&timeBegin, NULL); /* Get first state-action pair */ gettimeofday(&timeStart, NULL); /* ************************************************************************ * Control loop * ***********************************************************************/ while (TRUE) { ++iteration; if((iteration%100)==0) { printf("Iteration number: %6d\n",++iteration); printf("Time for iteration (in microseconds): %ld\n", computationTime); } gettimeofday(&timeEnd, NULL); computationTime = (timeEnd.tv_sec-timeStart.tv_sec)*1000000 + (timeEnd.tv_usec-timeStart.tv_usec); usleep(timestep - computationTime); gettimeofday(&timeStart, NULL); } return 0; }