// --- Main Program ----------------------------------
int main(void) {
//init PortC
//Pins C.1 to C.3 for controlling sonars
//Pins C.5, C.6 and C.7 for front, left and right motors respectively
DDRC = 0xff;
//init ADC
adc_init();
//init the UART -- trt_uart_init() is in trtUart.c
trt_uart_init();
stdout = stdin = stderr = &uart_str;
fprintf(stdout,"\n\r Start TRT\n\r\n\r");
// start TRT
trtInitKernel(80); // 80 bytes for the idle task stack
// --- create semaphores ----------
trtCreateSemaphore(SEM_RANGE, 1) ; // protect ranging
// --- create tasks ----------------
trtCreateTask(readSonar1, 500, SECONDS2TICKS(0.1), SECONDS2TICKS(0.1), &(args[0]));
trtCreateTask(readSonar2, 500, SECONDS2TICKS(0.1), SECONDS2TICKS(0.1), &(args[1]));
trtCreateTask(readSonar3, 500, SECONDS2TICKS(0.1), SECONDS2TICKS(0.1), &(args[2]));
trtCreateTask(navLogic, 400, SECONDS2TICKS(0.1), SECONDS2TICKS(0.1), &(args[3]));
trtCreateTask(sonar3Feedback, 400, SECONDS2TICKS(0.1), SECONDS2TICKS(0.1), &(args[0]));
// --- Idle task --------------------------------------
while (1)
{
// Do Nothing
}
}
// Read the right sonar
void readSonar2(void* args)
{
uint32_t rel, dead ;
uint8_t sonar_adc2;
while(1)
{
if (firstRun == 0) // not the first run of this task
{
trtWait(SEM_RANGE);
//turn on RX of sonar2
PORTC = PORTC ^ 0x04;
_delay_ms(50);
// read ADC1 and convert to meters
sonar_adc2 = adc_read(1);
sonarR_range = (float)sonar_adc2 * 0.0508;// (5/256)*(512/5)*.0254 * adc reading
//turn off RX
PORTC = PORTC ^ 0x04;
_delay_ms(20);
trtSignal(SEM_RANGE);
// Save ranges
rangeR[((iR++)%windowSize)] = sonarR_range;
} // end of -- if (firstRun == 0)
// Sleep
rel = trtCurrentTime() + SECONDS2TICKS(sonarFreq);
dead = trtCurrentTime() + SECONDS2TICKS(sonarFreq);
trtSleepUntil(rel, dead);
} // end of -- while(1)
}
//Display the Temperature on the LCD
void displayTemp(void* args)
{
//String constants
const uint8_t LCDTemp[5] = "TEMP\0";
//LCD locations
const uint8_t T_FOOD_REF_LOC = 0;
const uint8_t T_FOOD = 5;
uint8_t tempRefLen;
uint8_t tempLen;
float localTemp;
float localTempRef;
//String buffers
uint8_t LCDTempRef[4];
uint8_t LCDTempMeas[5];
//flags indicating whether a variable needs to be updated
uint8_t updateTempRefRef;
//make local copies of the system parametes
trtWait(SEM_T_REF);
localTempRef = waterTempRef;
trtSignal(SEM_T_REF);
trtWait(SEM_T);
localTemp = waterTemp;
trtSignal(SEM_T);
trtWait(SEM_THICKNESS);
float localThickness = thickness;
trtSignal(SEM_THICKNESS);
trtWait(SEM_MAT_PROP);
float localK = k;
trtSignal(SEM_MAT_PROP);
LCDGotoXY(0,0);
LCDstring(LCDTemp, 4);
uint32_t rel, dead;
while(1){
//trtWait(SEM_T_WATER);
//trtSignal(SEM_T_WATER);
sprintf(LCDTempMeas, "%f", localTemp);
LCDGotoXY(0, 0);
LCDstring(LCDTempMeas, 5);
rel = trtCurrentTime() + SECONDS2TICKS(0.2);
dead = trtCurrentTime() + SECONDS2TICKS(0.225);
trtSleepUntil(rel, dead);
}
}
void
arp_set_resolved(arp_entry_p e, char ip_addr[4], char eth_addr[6]) {
e->status = RESOLVED;
e->ticks = TICKS + SECONDS2TICKS(ARP_RESOLVED_TIMEOUT);
e->count = 0;
memcpy(e->ip, ip_addr, 4);
memcpy(e->eth, eth_addr, 6);
}
/* Retransmission, sends requests and decrements retry count */
void
arp_retransmit(arp_entry_p e) {
assert(EISPENDING(e));
assert(e->count > 0);
/* send_request(e) */
e->count--;
e->ticks = TICKS + SECONDS2TICKS(ARP_RESEND_TIMEOUT);
}
// Read Sonar3, i.e. fwd sonar
void readSonar3(void* args)
{
uint32_t rel, dead ;
uint8_t sonar_adc3;
while(1)
{
if (firstRun) // not the first run of this task
{
// Calibrate front sensor
calibrateFront();
firstRun = 0;
}
if (firstRun == 0) // not the first run of this task
{
trtWait(SEM_RANGE);
//turn on RX of sonar1
PORTC = PORTC ^ 0x08;
_delay_ms(50);
// read ADC0 and convert to meters
sonar_adc3 = adc_read(2);
sonarF_range = (float)sonar_adc3 * 0.0508; // (5/256)*(512/5)*.0254 * adc reading
//turn off RX
PORTC = PORTC ^ 0x08;
_delay_ms(20);
trtSignal(SEM_RANGE);
// Save ranges
rangeF[((iF++) % windowSize)] = sonarF_range;
} // end of -- if (fristRun == 0)
// Sleep
rel = trtCurrentTime() + SECONDS2TICKS(sonarFreq);
dead = trtCurrentTime() + SECONDS2TICKS(sonarFreq);
trtSleepUntil(rel, dead);
} // end of -- while(1)
}
int main(void)
{
initialize();
trt_uart_init();
/* Print a statement to the serial communication terminal when the system is reset. */
stdout = stdin = stderr = &uart_str;
fprintf(stdout,"TinyRealTime: 2/9/09\n\r");
/* Sets up the kernel data structures.
* The parameter is the desired starck size of the idle task.
* For a null idle task, a stack size of 80 should be sufficient. */
trtInitKernel(80);
/* Creates semaphores with identifer semnumber and initial values initval. */
trtCreateSemaphore(SEM_RX_ISR_SIGNAL, 0);
trtCreateSemaphore(SEM_STRING_DONE , 0);
trtCreateSemaphore(SEM_S , 1);
trtCreateSemaphore(SEM_P , 1);
trtCreateSemaphore(SEM_I , 1);
trtCreateSemaphore(SEM_D , 1);
/* Identifies the three functions to the kernel as threads.
* The parameters specify pointers to the functions, the
* desired stack size, the initial release time, the initial deadline time,
* and an abitrary data input structure */
trtCreateTask(serialTask , 200, SECONDS2TICKS(0.1), SECONDS2TICKS(0.2 ), &(args[0]));
trtCreateTask(motorTask , 200, SECONDS2TICKS(0.3), SECONDS2TICKS(0.32), &(args[1]));
/* Choose our preferred sleep mode */
set_sleep_mode(SLEEP_MODE_IDLE);
/* Set sleep enable (SE) bit */
sleep_enable();
/* Sleep the CPU when a task isn't running. */
while (1) {
sleep_cpu();
}
}
// front sonar haptic feedback
void sonar3Feedback(void* args)
{
uint32_t rel, dead ;
float duration = 0;
while(1)
{
if ((sonarF_range < F_calibrated) && (firstRun == 0))
{
duration = 1/sonarF_range * 20;
PORTC = PORTC ^ 0x20; // PINC5
_delay_ms(duration);
PORTC = PORTC ^ 0x20;
}
// Sleep
rel = trtCurrentTime() + SECONDS2TICKS(sonar3FeedbackFreq);
dead = trtCurrentTime() + SECONDS2TICKS(sonar3FeedbackFreq);
trtSleepUntil(rel, dead);
}
}
// --- Main Program ----------------------------------
int main(void) {
int args[2];
DDRD = 0b11111011;
PORTD = 0;
//Initialize the MCU
initialize();
// start TRT
trtInitKernel(128); // 80 bytes for the idle task stack
// variable protection
trtCreateSemaphore(SEM_T_REF, 1) ; // protect shared variables
trtCreateSemaphore(SEM_T, 1) ; // protect shared variables
trtCreateSemaphore(SEM_T_WATER_REF, 1) ; // protect shared variables
trtCreateSemaphore(SEM_T_WATER, 1) ; // protect shared variables
trtCreateSemaphore(SEM_MAT_PROP, 1) ; // protect shared variables
trtCreateSemaphore(SEM_THICKNESS, 1);
// --- creat tasks ----------------
trtCreateTask(pidControl, 256, SECONDS2TICKS(0.05), SECONDS2TICKS(0.05), &(args[0]));
trtCreateTask(keypadComm, 256, SECONDS2TICKS(0.1), SECONDS2TICKS(0.1), &(args[1]));
trtCreateTask(displayTemp, 256, SECONDS2TICKS(0.1), SECONDS2TICKS(0.1), &(args[1]));
sei();
// --- Idle task --------------------------------------
// just sleeps the cpu to save power
// every time it executes
set_sleep_mode(SLEEP_MODE_IDLE);
sleep_enable();
while (1)
{
sleep_cpu();
}
}
void motorTask(void* args)
{
uint32_t rel, dead;
float motor_rpm = 0; /* Actual motor RPM. */
float pErr = 0; /* Proportional gain error term. */
float iErr = 0; /* Integral gain error term. */
float dErr = 0; /* Differential gain error term. */
float output; /* Summation of the three aforementioned error terms. */
while(1)
{
/* Calculation of the actual motor RPM. */
motor_rpm = 60/(7*((float) motor_period)/F_CPU);
/* Calculation of the deviation between the desired and actual fan speed. */
err = s - motor_rpm;
if (err > 0) {
iErr += err;
}
else {
iErr *= iFact;
}
dErr = err - pErr;
/* Summation of the three aforementioned error terms. */
output = p*err + i*iErr + d*dErr;
/* PWM will drive the motor at 12V, which is full speed. */
if (output > 255) {
OCR0A = 255;
}
/* PWM will drive the motor at 0V, which shuts it off. */
else if (output < 0) {
OCR0A = 0;
}
/* PWM will drive the motor at the desired rate. */
else {
OCR0A = (unsigned char) output;
}
pErr = err; // Save current error as previous
/* Ensure there is a maximum bound on the motor RPM. */
if (motor_rpm > 3269) {
OCR0B = 255;
}
/* Ensure there is a minimum bound on the motor RPM. */
else if (motor_rpm < 0) {
OCR0B = 0;
}
/* Scale the motor RPM to a value between 0 and 255. */
else {
OCR0B = (unsigned char)(0.078 * motor_rpm);
}
rel = trtCurrentTime() + SECONDS2TICKS(0.01);
dead = trtCurrentTime() + SECONDS2TICKS(0.01);
trtSleepUntil(rel, dead);
}
}
//Actual tasks to complete
//PID Control Stuff...worry about this silt later
// --- define task 1 ----------------------------------------
void pidControl(void* args)
{
uint32_t rel, dead ; //relase and deadline times
float error = 0; //Calculated error
float prevError = 0; //previously calculated error
float prevWaterTemp = 0; //previously measured motor frequency
int8_t prevSign = 0; //previously calculated sign of the error
//Local copies of shared system parameters
uint16_t localWaterTemp;
uint16_t localWaterTempRef;
//PID parameters
float k_p = 1.0, k_i = 0.0, k_d = 0.0;
//Declarations for calculated values.
int8_t sign = 0;
int16_t derivative;
int16_t output;
int16_t integral = 0;
uint8_t first = 1;
//transduction constant for the LM34
const float transductionConstant = 0.01; // V/degF
while(1)
{
//update the previous measuremtns
if (!first){
prevWaterTemp = localWaterTemp;
prevSign = sign;
prevError = error;
}
//poll the ADC and convert the voltage to a temperature
Ain = ADCH;
ADCSRA |= (1<<ADSC); //start another conversion
voltage = (float)Ain;
voltage = (voltage/256.0) * Vref;
localWaterTemp = voltage * transductionConstant;
//copy to global waterTemp
trtWait(SEM_T_WATER);
waterTemp = localWaterTemp;
trtSignal(SEM_T_WATER);
//make local copies of the system parameters
trtWait(SEM_T_WATER_REF);
localWaterTempRef = waterTempRef;
trtSignal(SEM_T_WATER_REF);
//Proportional Error
error = localWaterTempRef - localWaterTemp;
//Integral Error
//Get the current sign of the error
if (!first) {
if (error - prevError > 0){
sign = 1;
}
else if (error - prevError < 0) {
sign = -1;
}
else {
sign = 0;
}
}
//Update the integral of the error
if (!first){
if (sign == prevSign){
integral += error;
}
else{
integral = 0.8 * error;
}
}
//Derivative Error
if (!first) {
derivative = error - prevError;
}
//determine what the output should be
if (!first){
output = k_p * error + k_i * integral + k_d * derivative;
}
else{
output = k_p * error;
first = 0;
}
//clamp the output between 0 and 255 so we can directly set OCR0A
if (output < 0){
OCR0A = 0;
}
else if (output > 255) {
OCR0A = 255; //saturated the controller, turn the integrator off
integral = 0;
}
else {
OCR0A = output;
}
if (error > 0){
//Turn the heating thing on
PORTA |= 0x08; //Pin 3...???
}
else{
//Turn the heating thing off
PORTA &= ~0x08;
}
ADCSRA |= (1<<ADSC);
//Set the task to execute again in 0.02 seconds.
rel = trtCurrentTime() + SECONDS2TICKS(0.19);
dead = trtCurrentTime() + SECONDS2TICKS(0.21);
trtSleepUntil(rel, dead);
}
}
void
arp_set_timeout(arp_entry_p e) {
e->status = TIMEOUT;
e->ticks = TICKS + SECONDS2TICKS(ARP_TIMEOUT_TIMEOUT);
e->count = 0;
}
// --- Main Program ----------------------------------
int main(void) {
DDRD = 0b11111011;
PORTD = 0;
//init the UART -- trt_uart_init() is in trtUart.c
trt_uart_init();
stdout = stdin = stderr = &uart_str;
fprintf(stdout,"\n\r TRT 9feb2009\n\r\n\r");
//enable ADC and set prescaler to 1/128*16MHz=125,000
//and clear interupt enable
//and start a conversion
ADCSRA = (1<<ADEN) + 7;
// Set analog comp to connect to timer capture input
// and turn on the band gap reference on the positive input
ACSR = (1<<ACIC) ; //0b01000100 ;
//initialize Timer2 and the external interrupt
//set up INT0
EIMSK = 1<<INT1 ; // turn on int0
EICRA = 1 << ISC10 ; // trigger on any edge edge
// turn on timer 2 to be read in int0 ISR
TCCR2B = 7 ; // divide by 1024
// turn on timer 2 overflow ISR for double precision time
TIMSK2 = 1;
//setup Timer 0
// Set the timer for fast PWM mode, clear OC0A on Compare Match, set OC0A
// at BOTTOM (non-inverting mode)
TCCR0A = (1 << COM0A1) | (1 << WGM01) | (1 << WGM00); //Set the timer
//Set the prescalar to 256 so the PWM runs at less than 1000 Hz
TCCR0B = (1 << CS02) | (1 << CS00);
OCR0A = 127;
// start TRT
trtInitKernel(128); // 80 bytes for the idle task stack
// --- create semaphores ----------
// You must creat the first two semaphores if you use the uart
trtCreateSemaphore(SEM_RX_ISR_SIGNAL, 0) ; // uart receive ISR semaphore
trtCreateSemaphore(SEM_STRING_DONE,0) ; // user typed <enter>
// variable protection
trtCreateSemaphore(SEM_TEMP_REF, 1) ; // protect shared variables
trtCreateSemaphore(SEM_TEMP, 1) ; // protect shared variables
trtCreateSemaphore(SEM_OVERFLOW, 1) ; // protect shared variables
trtCreateSemaphore(SEM_THICKNESS, 1) ; // protect shared variables
trtCreateSemaphore(SEM_START_HEATING, 0) ; // protect shared variables
trtCreateSemaphore(SEM_SONAR, 1); // Condition Variable for sonar measurement
// --- creat tasks ----------------
trtCreateTask(pidControl, 256, SECONDS2TICKS(0.05), SECONDS2TICKS(0.05), &(args[0]));
// trtCreateTask(serialComm, 256, SECONDS2TICKS(0.1), SECONDS2TICKS(0.1), &(args[1]));
// trtCreateTask(displayParams, 256, SECONDS2TICKS(0.1), SECONDS2TICKS(0.1), &(args[1]));
trtCreateTask(sonar, 256, SECONDS2TICKS(0.05), SECONDS2TICKS(0.05), &(args[0]));
sei();
// --- Idle task --------------------------------------
// just sleeps the cpu to save power
// every time it executes
set_sleep_mode(SLEEP_MODE_IDLE);
sleep_enable();
while (1)
{
sleep_cpu();
}
} // main
//PID Control Stuff keep the water temperature constant
// --- define task 1 ----------------------------------------
void pidControl(void* args)
{
uint32_t rel, dead ; //relase and deadline times
int16_t error = 0; //Calculated error
int16_t prevError = 0; //previously calculated error
uint16_t prevTemp; //previously measured motor frequency
int8_t prevSign =0 ; //previously calculated sign of the error
//Local copies of shared system parameters
uint8_t localTemp = 0;
uint8_t localTempRef;
uint8_t localWaterTemp;
uint8_t localWaterTempRef;
float k_p = 7.1;
float k_i = 0.11;
float k_d = 0.68;
//transduction constant for LM35
float transductionConstant = 0.1;
//Declarations for calculated values.
int8_t sign = 0;
int16_t derivative;
int16_t output;
int16_t integral = 0;
uint8_t first = 1;
DDRB = 0xff;
PORTB = 0;
while(1)
{
while (!startHeating) {
trtWait(SEM_START_HEATING);
}
Ain = ADCH;
voltage = (float)Ain;
voltage = (voltage/256.0) * Vref;
localWaterTemp = voltage * transductionConstant
//update the previous measuremtns
if (!first) {
prevTemp = localTemp;
prevSign = sign;
prevError = error;
}
localWaterTemp = 0;
//make local copies of the system parameters
trtWait(SEM_TEMP);
waterTemp = localWaterTemp;
trtSignal(SEM_TEMP);
trtWait(SEM_TEMP_REF);
localWaterTempRef = waterTempRef;
trtSignal(SEM_TEMP_REF);
//Proportional Error
error = localWaterTempRef - localWaterTemp;
//Integral Error
//Get the current sign of the error
if (!first) {
if (error - prevError > 0) {
sign = 1;
}
else if (error - prevError < 0) {
sign = -1;
}
else {
sign = 0;
}
}
//Update the integral of the error
if (!first) {
if (sign == prevSign) {
integral += error;
}
else {
integral = 0;
}
}
//Derivative Error
if (!first) {
derivative = error - prevError;
}
//determine what the output should be
if (!first) {
output = k_p * error + k_i * integral + k_d * derivative;
}
else {
output = k_p * error;
first = 0;
}
//clamp the output between 0 and 255 so we can directly set OCR0A
if (output < 0) {
OCR0A = 0;
}
else if (output > 255) {
OCR0A = 255;
}
else {
OCR0A = output;
}
ADCSRA |= (1<<ADSC); //start another converstion
//Set the task to execute again in 0.02 seconds.
rel = trtCurrentTime() + SECONDS2TICKS(0.02);
dead = trtCurrentTime() + SECONDS2TICKS(0.025);
trtSleepUntil(rel, dead);
}
}
// --- define a task for making sonar measurements
void sonar(void* args) {
uint32_t rel, dead ; //relase and deadline times
uint8_t threshold = 5; //cm, If a new value differs by this much, assume the
//food was put in
uint8_t refHeight = 0; //cm, the (final) height of the water without food
uint8_t newHeight = 0; //cm, new height of the bath after adding food
uint8_t heightAvg = 0; //cm, average height of the water. without food
uint8_t newHeightSet = 0; //Flag indicating that the thickness of the food
//has been calculated
uint8_t refHeightSet = 0; //Flag indicating that we have measured
//the reference distance from the sonar
//to the water's surface
//Boundaries so we don't measure the top or bottom of the bath
uint8_t minDist = 3; //cm
uint8_t maxDist = 30; //cm
uint8_t numSamples = 0; //he number of samples currently taken in the set
uint8_t desiredNumSamples = 64; //the number of samples to take in a set.
uint8_t prevDistance = 0; //cm, the previous measurement
//Ratio of the area of the bath and cooking bags
uint8_t areaRatio = 2;
uint8_t firstMeasurementTaken = 0; //flag indicating that the very first measurement has not been taken
uint8_t firstSample = 0; //cm, first measurement out of 64 in a set of samples
//flags inicating which distance we are measuring
uint8_t measuringRefHeight = 1;
uint8_t measuringNewHeight = 0;
while(1) {
PORTA |= 0x01; //Set the trigger pin high to start the pulse
_delay_us(10); //Give the ultrasonic 10 us to send the pulse
PORTA &= ~0x01; //Set the trigger pin low to stop transmitting
sonarFinished = 0;
//Use TRTAccept to read the sonarFinishedSemaphor
while (!sonarFinished) {
trtWait(SEM_SONAR);
}
numSamples++;
//if this is the first sample, just record the distance
//as the height, previous measurement, and first sample.
if (!firstMeasurementTaken) {
prevDistance = distance;
heightAvg = distance;
firstMeasurementTaken = 1;
firstSample = distance;
}
//Otherwise determine what we wanted to measure
//and whether or not the state ofthe cooker has changed
else {
//check if the measurement is valid.
if (distance < maxDist and distance > minDist) {
//check if the depth increased by more than 5cm (corresponds to
// adding or removing food).
if (distance - prevDistance > threshold or prevDistance - distance > 5) {
numSamples = 1;
firstSample = distance;
newHeight = distance;
measuringNewHeight = 1;
measuringRefHeight = 0;
}
else {
if (!measuringNewHeight) {
if (!numSamples) {
firstSample = distance;
heightAvg = 0;
}
heightAvg += distance;
}
else {
newHeight += distance;
}
numSamples++;
}
//check if we are done sampling
if (numSamples == desiredNumSamples) {
//check if we are setting the reference height
//or the new height
if (!measuringNewHeight) {
heightAvg <<= 6;
//check if the measurement has increased substantially
//since taking the first sample. If so, the water level
//is rising and we should start sampling again
if (heightAvg - firstSample > threshold or firstSample - heightAvg > threshold) {
measuringRefHeight = 1;
numSamples = 0;
}
//the water level has settled. Set the reference height flag
//and clear the measuring reference height flag
else {
if (measuringRefHeight) {
refHeight = heightAvg;
refHeightSet = 1;
measuringRefHeight = 0;
}
numSamples = 0;
}
}
//We finished taking measurements for the new height
//of the bath
else {
//set the new height of the bath, calculate the thickness
//of the food, and signal the heating task to start heating
//the water
newHeight <<= 6;
newHeightSet = 1;
measuringNewHeight = 0;
trtWait(SEM_THICKNESS);
thickness = areaRatio * (newHeight - refHeight);
trtSignal(SEM_THICKNESS);
trtSignal(SEM_START_HEATING);
numSamples =0 ;
}
}
}
else if (distance <= minDist) {
overflow = 1;
}
else {
firstMeasurementTaken = 0;
measuringNewHeight = 0;
measuringRefHeight = 1;
numSamples = 0;
}
prevDistance = distance;
}
rel = trtCurrentTime() + SECONDS2TICKS(0.015625);
dead = trtCurrentTime() + SECONDS2TICKS(0.02);
trtSleepUntil(rel, dead);
}
}
// Navigation logic
// send haptic feedback depending on distance from sensors
void navLogic(void* args)
{
// where is the object?
#define NONE 0
#define LEFT 1
#define RIGHT 2
int L_thresh = 2;
int R_thresh = 2;
uint8_t state = NONE;
uint32_t rel, dead ;
while(1)
{
if (firstRun == 0) // not the first run of this task
{
// Median filter
if (iL >= 3) sonarL_range = medianFilter(rangeL);
if (iR >= 3) sonarR_range = medianFilter(rangeR);
if (iF >= 3) sonarF_range = medianFilter(rangeF);
// obstacle found within threshold
if ((sonarL_range < L_thresh) || (sonarR_range < R_thresh))
{
if (sonarL_range < sonarR_range) state = LEFT;
else if (sonarR_range < sonarL_range) state = RIGHT;
}
else
{
state = NONE;
}
switch (state)
{
case LEFT:
{
float durationL = 1/sonarL_range * 30;
//pulse left
PORTC = PORTC ^ 0x40; // PIN C6
//_delay_ms(90);
_delay_ms(durationL);
PORTC = PORTC ^ 0x40;
state = NONE;
} break;
case RIGHT:
{
float durationR = 1/sonarR_range * 30;
//pulse right
PORTC = PORTC ^ 0x80; // PIN C7
//_delay_ms(90);
_delay_ms(durationR);
PORTC = PORTC ^ 0x80;
state = NONE;
} break;
default:
{} break;
} // end of -- switch (state)
} // end of -- if (firstRun == 0)
// Sleep
rel = trtCurrentTime() + SECONDS2TICKS(navFreq);
dead = trtCurrentTime() + SECONDS2TICKS(navFreq+.1);
trtSleepUntil(rel, dead);
} // end of -- while
}