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