// 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 trtTaskDump(char tsk, char freeze_timers)
{
char timer0_tccr0b=0, timer1_tccr1b=0, timer2_tccr2b=0 ;
char i ;
char start, stop;
if (freeze_timers)
{
cli(); // disable interrupts
// and stop timers
timer0_tccr0b = TCCR0B ;
TCCR0B = 0 ;
timer1_tccr1b = TCCR1B ;
TCCR1B = 0 ;
timer2_tccr2b = TCCR2B ;
TCCR2B = 0 ;
sei();
}
fprintf(stdout, "# state relT deadT stkfree stkfreeMin\n\r") ;
if (tsk == 0)
{
start = 1;
stop = kernel.nbrOfTasks;
}
else
{
start = tsk;
stop = tsk;
}
//loop thru all tasks
for (i=start; i <= stop; i++)
{
fprintf(stdout, "%1d %4d %8ld %8ld %5d %5d\n\r",
i,
trtTaskState(i),
trtTaskRelease(i) - trtCurrentTime(),
trtTaskDeadline(i) - trtCurrentTime(),
trtTaskStack(i) - trtTaskStackBottom(i),
trtTaskStackFreeMin(i) ) ;
}
if (freeze_timers)
{
// restore timer state
TCCR0B = timer0_tccr0b ;
TCCR1B = timer1_tccr1b ;
TCCR2B = timer2_tccr2b ;
}
}
// 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)
}
// 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);
}
}
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);
}
}
//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
}
