//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 serialTask(void* args)
{
char cmd[4];
float num = 0;
while(1)
{
fscanf(stdin, "%s %f", cmd, &num);
if (cmd[0] == 's') {
/* Set desired motor speed */
trtWait (SEM_S);
s = num;
trtSignal(SEM_S);
/* Statment confirming the new speed is printed. */
fprintf(stdout, "\n\rDesired motor speed is now %.2f\n\r\n\r", (double) num);
}
else if (cmd[0] == 'p') {
/* Set proportional gain */
trtWait (SEM_P);
p = num;
trtSignal(SEM_P);
/* Statment confirming the new proportional gain is printed. */
fprintf(stdout, "\n\rProportional gain is now %.2f\n\r\n\r", (double) num);
}
else if (cmd[0] == 'i') {
/* Set integral gain */
trtWait (SEM_I);
i = num;
trtSignal(SEM_I);
/* Statment confirming the new integral gain is printed. */
fprintf(stdout, "\n\rIntegral gain is now %.2f\n\r\n\r", (double) num);
}
else if (cmd[0] == 'd') {
/* Set differential gain */
trtWait (SEM_D);
d = num;
trtSignal(SEM_D);
/*Statment confirming the new differential gain is printed.*/
fprintf(stdout, "\n\rDifferential gain is now %.2f\n\r\n\r", (double) num);
}
}
}
// 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)
}
//==============================================================
// trtLockMutex
// LOCK and record owner
// If already LOCKED, force a context switch
void trtLockMutex(uint8_t mutex_number){
cli();
// if not locked
if (mutex[mutex_number].state == UNLOCKED){
mutex[mutex_number].state = LOCKED ;
mutex[mutex_number].owner = kernel.running ;
}//if (tmutex[mutex_number].state = UNLOCKED)
else if (mutex[mutex_number].owner != kernel.running){
trtWait(mutex_number) ;
}
sei();
} //void trtLockMutex(uint8_t mutex_number)
// 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)
}
//get input from the keypad
void keypadComm(void* args) {
//uint8_t cmd0, cmd1, cmd2, cmd3;
//float val0, val1, val2, val3;
//uint8_t numParams;
uint8_t key = getCurKey();
uint8_t valBuffer[16]; // Input parameter buffer
uint32_t valInt = 0; // Input parameter - before the decimal place
uint32_t valDec = 0; // Input parameter after the decimal place
uint32_t numAfterDecimal = 1;
uint8_t valLoc = 0;
float val; //The actual input value
uint8_t beforeDecimal = 1; //flag indicating whether or not the current
//key comes before or after the decimal point
while(1)
{
//If we aren't debouncing, scan the keypad and begin debouncing the signal
if (!debouncing){
debounce();
}
//Otherwise, finish debouncing the signal
else {
debounce();
}
if (waitingForInput) {
switch (key){
case 0x01:
valBuffer[valLoc] = '1';
if (beforeDecimal){
valInt = valInt * 10 + 1;
}
else {
valDec = valDec * 10 + 1;
numAfterDecimal *= 10;
}
valLoc++;
break;
case 0x02:
valBuffer[valLoc] = '2';
if (beforeDecimal) {
valInt = valInt * 10 + 2;
}
else {
valDec = valDec * 10 + 2;
numAfterDecimal *= 10;
}
valLoc++;
break;
case 0x03:
valBuffer[valLoc] = '3';
if (beforeDecimal) {
valInt = valInt * 10 + 3;
}
else{
valDec = valDec * 10 + 3;
numAfterDecimal *= 10;
}
valLoc++;
break;
case 0x04:
valBuffer[valLoc] = '4';
if (beforeDecimal) {
valInt = valInt * 10 + 4;
}
else{
valDec = valDec * 10 + 4;
numAfterDecimal *= 10;
}
valLoc++;
break;
case 0x05:
valBuffer[valLoc] = '5';
if (beforeDecimal) {
valInt = valInt * 10 + 5;
}
else{
valDec = valDec * 10 + 5;
numAfterDecimal *= 10;
}
valLoc++;
break;
case 0x06:
valBuffer[valLoc] = '6';
if (beforeDecimal) {
valInt = valInt * 10 + 6;
}
else{
valDec = valDec * 10 + 6;
numAfterDecimal *= 10;
}
valLoc++;
break;
case 0x07:
valBuffer[valLoc] = '7';
if (beforeDecimal) {
valInt = valInt * 10 + 7;
}
else{
valDec = valDec * 10 + 7;
numAfterDecimal *= 10;
}
valLoc++;
break;
case 0x08:
valBuffer[valLoc] = '8';
if (beforeDecimal) {
valInt = valInt * 10 + 8;
}
else{
valDec = valDec * 10 + 8;
numAfterDecimal *= 10;
}
valLoc++;
break;
case 0x09:
valBuffer[valLoc] = '9';
if (beforeDecimal) {
valInt = valInt * 10 + 9;
}
else{
valDec = valDec * 10 + 9;
numAfterDecimal *= 10;
}
valLoc++;
break;
case 0x10:
valBuffer[valLoc] = '0';
if (beforeDecimal) {
valInt = valInt * 10;
}
else{
valDec = valDec * 10;
numAfterDecimal *= 10;
}
valLoc++;
break;
case 0x0E: //E ==> decimal
valBuffer[valLoc] = '.';
valLoc++;
beforeDecimal = 0;
case 0x0F: //F ==> backspace
valLoc--;
if (beforeDecimal){
valInt = valInt / 10;
}
else {
if (valBuffer[valLoc] == '.'){
valInt = valInt / 10;
beforeDecimal = 1;
numAfterDecimal = 1;
}
else {
valDec = valDec / 10;
numAfterDecimal /= 10;
}
}
case 0x0D: //D ==> enter
waitingForInput = 0;
val = valInt + (float)valDec / (float)numAfterDecimal;
switch (mode){
case INPUT_T_REF:
trtWait(SEM_T_WATER_REF);
waterTempRef = val;
trtSignal(SEM_T_WATER_REF);
break;
case INPUT_THICKNESS:
trtWait(SEM_THICKNESS);
thickness = val;
trtSignal(SEM_THICKNESS);
break;
case INPUT_MAT_PROP:
trtWait(SEM_MAT_PROP);
k = val;
trtSignal(SEM_MAT_PROP);
break;
}
break;
}
}
}
}
//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);
}
}
int
uart_getchar(FILE *stream)
{
uint8_t c;
char *cp, *cp2;
static char b[RX_BUFSIZE];
static char *rxp;
if (rxp == 0)
for (cp = b;;)
{
// --- trtWait added instead of loop_until wait
trtWait(SEM_RX_ISR_SIGNAL) ; //loop_until_bit_is_set(UCSR0A, RXC0)
if (UCSR0A & _BV(FE0))
return _FDEV_EOF;
if (UCSR0A & _BV(DOR0))
return _FDEV_ERR;
// -- added to take char from ISR ---
c = trt_rx_c ; //c = UDR0; -- CHANGED
/* behaviour similar to Unix stty ICRNL */
if (c == '\r')
c = '\n';
if (c == '\n')
{
*cp = c;
uart_putchar(c, stream);
rxp = b;
// --- added for TRT to signal string-end
trtSignal(SEM_STRING_DONE); //added--to signal end of string
break;
}
else if (c == '\t')
c = ' ';
if ((c >= (uint8_t)' ' && c <= (uint8_t)'\x7e') ||
c >= (uint8_t)'\xa0')
{
if (cp == b + RX_BUFSIZE - 1)
uart_putchar('\a', stream);
else
{
*cp++ = c;
uart_putchar(c, stream);
}
continue;
}
switch (c)
{
case 'c' & 0x1f:
return -1;
case '\b':
case '\x7f':
if (cp > b)
{
uart_putchar('\b', stream);
uart_putchar(' ', stream);
uart_putchar('\b', stream);
cp--;
}
break;
case 'r' & 0x1f:
uart_putchar('\r', stream);
for (cp2 = b; cp2 < cp; cp2++)
uart_putchar(*cp2, stream);
break;
case 'u' & 0x1f:
while (cp > b)
{
uart_putchar('\b', stream);
uart_putchar(' ', stream);
uart_putchar('\b', stream);
cp--;
}
break;
case 'w' & 0x1f:
while (cp > b && cp[-1] != ' ')
{
uart_putchar('\b', stream);
uart_putchar(' ', stream);
uart_putchar('\b', stream);
cp--;
}
break;
}
}
c = *rxp++;
if (c == '\n')
rxp = 0;
return c;
}
//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);
}
}
