Blog 6

 Introduction

As the last blog post of this semester, this post will explain my understanding of the state machine in the Sweeper robot code as a member of the team, the problems we encountered, and areas for future work and improvement.

ADC Value Mapping

After getting the ADC readings from the sensors, we noticed that the values were not very stable and would sometimes fluctuate. Because of this, using the raw ADC values directly caused unreliable behaviour in the system.

To improve this, we applied simple thresholding to constrain the ADC values within a reasonable range. After selecting the ADC channel and reading the value, we limited extreme readings by applying upper and lower bounds:

set_adc_channel(1);
delay_ms(1);
adc_value1 = read_adc();

if(adc_value1 > 120)
{
    adc_value1 = 120;
}
if(adc_value1 < 20)
{
    adc_value1 = 20;
}

This keeps the ADC values within a usable window (20–120), removing extreme spikes that are mainly caused by noise rather than actual sensor input.

In addition to this basic clipping, further thresholds were also used when comparing multiple sensor readings. For example, when determining direction, a margin (e.g. a difference of 20) is introduced to avoid false triggering due to small variations:

else if (adc_value1 > adc_value2 + 20)
{
    // LEFT
}
else if (adc_value2 > adc_value1 + 20)
{
    // RIGHT
}

This means the system does not react to very small differences between sensors, which improves stability when deciding movement direction.

State Machine Diagram

The state machine controls how the robot searches for and follows the box in a fairly intuitive way. When the system starts, it immediately enters Radar mode, where the robot rotates on the spot. The idea here is simple: instead of moving blindly, it scans its surroundings until both sensors detect something near it.

While in Radar, the robot keeps turning until both sensor readings become similar (or within a small margin). This indicates that the object is roughly centred in front of the robot. At this point, the system switches into Chase mode, where both motors are driven forward, allowing the robot to move straight towards the box.

Once in Chase, the robot continuously compares the two sensor values. If one sensor starts reading significantly higher than the other, it means the box is slightly off to one side. The robot then adjusts by entering either Chase Left or Chase Right, where it deliberately biases the motor speeds to turn back towards the target. In this sense, the robot is constantly “correcting” its direction while moving forward.

An interesting behaviour occurs when the robot loses the box during a turn. Instead of stopping or resetting immediately, it continues turning in the same direction it was already moving. This creates a kind of “search while turning” behaviour, which helps the robot reacquire the box more quickly without going back to a full scan. Once both sensors detect the box again, the system returns to Chase mode and continues moving forward.

Future Work

Future work could explore giving the robot a sense of ''reset''. After completing a chase, instead of continuously searching nearby, it could return to a known starting point before re-entering Radar mode. This would allow it to begin each search from a consistent position, potentially improving tracking reliability and overall behaviour.

Acknowledgements

First of all, a big thank you to Jason for giving us the chance to work together as a team, tackle challenges, and bring this project to life. It was a great experience, and we'll definitely remember creating what we like to call ''the world's best state machine diagram''.

I'd also like to thank everyone in the team — this project wouldn't have been possible without the effort, support, and ideas from each of you. Working together like this has been both enjoyable and meaningful, and it's something I'll carry with me going forward.

Finally, thanks again to Jason, Jake, Cuba, Yoma, Sean, Qimeng, and Tao.

Video:

Source Code:

#include <16f877a.h>
#device *=16 ADC=8
#use delay(clock=20000000)
#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7,ERRORS,DISABLE_INTS)
#use rtos(timer=1,minor_cycle=100ms,)

#include <stdio.h>
#include <stdlib.h>
#include "BENGLCD420.C"
//#include <stdint.h>

#define RED PIN_B5
#define GREEN PIN_A5

#include <string.h>

// this character array will be used to take input from the prompt
char input [10];
// this will hold the current position in the array
int index;
// this will signal to the kernal that input is ready to be processed
int8 input_ready;

int8 task1_toggle=0;
int8 task2_toggle=0;
int8 task3_toggle=0;
int8 task4_toggle=0;
int8 task5_toggle=0;

//input pin pressed
int8 pressed =0;

//pwm signals for motors
int16 pwm1 = 1500; //1500us 1.5ms pwm pinb3
int16 pwm2 = 1500; //1500us 1.5ms pwm pinb4

//inputs
//!#define INPUT_1_ON 1
//!#define BOX 2
//!#define BOX_GONE 3
//!#define BOX_LEFT 4
//!#define BOX_RIGHT 5

//#define INPUT_1_ON      1
//#define INPUT_2_ADC_GREATER_THAN_100 2

//states
#define START 0
#define RADAR 1
#define CHASE 2
#define CHASE_LEFT 3
#define CHASE_RIGHT 4
//!#define STATE_5 5

#define BOX_GONE 0
#define BOX 1
#define BOX_LEFT 2
#define BOX_RIGHT 3

//#define STATE_1 1state machine input
unsigned int8 state_machine_input1 = 0;
//unsigned int8 state_machine_input2 = 0;

//state machine state variable
unsigned int8 state = START;

unsigned int8 input_task2 = 0;
int ctrl = 0;
int8 adc_value1 = 0;
int8 adc_value2 = 0;



//#task(rate=200ms,max=100ms)
//void input1_rtos_task ( );

#task(rate=200ms,max=100ms)
void input2_rtos_task ( );

#task(rate=200ms,max=100ms,queue=2)
void output1_rtos_task ( );

#task(rate=200ms,max=100ms,queue=2)
void output2_rtos_task ( );

#task(rate=100ms,max=100ms,queue=2)
void state_machine_rtos_task ( );


//!void input1_rtos_task ( )
//!{
//!
//!
//!int8 input_task1;
//!
//!while(1)
//!{
//!
//!//send value of switch
//!if( (!input(PIN_A4)) && (pressed == 0) ) // pin is active high low
//!{
//!pressed = 1;
//!
//!input_task1 = INPUT_1_ON;
//!//rtos_msg_send(state_machine_rtos_task,input_task1);
//!
//!}
//!else if(input(PIN_A4))
//!{
//!pressed = 0;
//!}
//!
//!//task debud info on lcd
//!if(task1_toggle == 0)
//!{
//!lcd_gotoxy( 1,1);
//!printf(lcd_putc,"in1 >>%u ",pressed);
//!task1_toggle = 1;
//!}
//!else
//!{
//!lcd_gotoxy( 1,1);
//!printf(lcd_putc,"in1 >>>");
//!task1_toggle = 0;
//!}
//!
//!rtos_yield();
//!}
//!}

void input2_rtos_task ( )
{
//int8 input_task2;
//int8 adc_value1 = 0;
//int8 adc_value2 = 0;
//!int8 box;
//!int8 box_gone1;
//!int8 box_gone2;
//!int8 box_left;
//!int8 box_right;
//!int8 default1;
//!int8 default2;

set_adc_channel(1);
delay_ms(1);

adc_value1 = read_adc();

if(adc_value1 > 120)
{
adc_value1 = 120;
}
if(adc_value1 < 20)
{
adc_value1 = 20;
}

set_adc_channel(2);
delay_ms(1);
adc_value2 = read_adc();

if(adc_value2 > 120)
{
   adc_value2 = 120;
}

if(adc_value2 < 20)
{
   adc_value2 = 20;
}

// Box gone
if (adc_value1 <= 20 && adc_value2 <= 20)
{
   //adc_value1 = 170;
   //adc_value2 = 150;
   //box_gone1 = adc_value1;
   //box_gone2 = adc_value2;
   //rtos_msg_send(state_machine_rtos_task, box_gone1);
   //rtos_msg_send(state_machine_rtos_task, box_gone2);
   rtos_msg_send(state_machine_rtos_task, BOX_GONE);
   printf(" MESSAGE:%u\n\r", BOX_GONE);
}
else if (adc_value1 > 20 && adc_value2 > 20) // change values?
{
   rtos_msg_send(state_machine_rtos_task, BOX);
   printf(" MESSAGE:%u\n\r", BOX);
  //adc_value1 + 80;
  //adc_value2 + 80;
}

else if (adc_value1 > adc_value2 + 20) // change values
{
   rtos_msg_send(state_machine_rtos_task, BOX_LEFT);
   printf(" MESSAGE:%u\n\r", BOX_LEFT);
}

else if (adc_value2 > adc_value1 + 20) // change values
{
   rtos_msg_send(state_machine_rtos_task, BOX_RIGHT);
   printf(" MESSAGE:%u\n\r", BOX_RIGHT);
}

//printf("adc box_gone1: %u\n\r", box_gone1);
//printf("adc box_gone2: %u\n\r", box_gone2);
//printf("adc default1: %u\n\r", default1);
//printf("adc default2: %u\n\r", default2);

// 3. Send processed value
//rtos_msg_send(state_machine_rtos_task, default1);
//rtos_msg_send(state_machine_rtos_task, default2);



// LCD debug unchanged...
}

void output1_rtos_task ()
{
while(1)
{

if(rtos_msg_poll ( )>0)
{
pwm1 = rtos_msg_read ( );
// the function rtos_msg_read, reads the first value in the queue
//printf("messages recieved by out1 : %lu\n\r",pwm1);

pwm1 = pwm1 * 10; // turn 200 into 2000 for us, min should be 100
//printf("messages recieved by out1 : %lu\n\r",pwm1);

// the funciton rtos_msg_send, sends the value given as the
// second parameter to the function given as the first
}


//pwm 1 drive motor 1 on robot pin 36 on pic
output_bit(PIN_B3,1);
delay_us(pwm1);
output_bit(PIN_B3,0);





//task debud info on lcd
////////////////////////
if(task3_toggle == 0)
{
lcd_gotoxy( 1,2);
printf(lcd_putc,"out1 >> ");
task3_toggle = 1;
}
else
{
lcd_gotoxy( 1,2);
printf(lcd_putc,"out1 >>>");
task3_toggle = 0;
}

rtos_yield();
}//end of while 1
}



void output2_rtos_task()
{
unsigned int8 message = 0;

//some reason message ques does not work for another output
// so we will pass to it using a protected variable
while(1)
{
if(rtos_msg_poll ( )>0)
{
pwm2 = rtos_msg_read ( );

// the function rtos_msg_read, reads the first value in the queue
//printf("messages recieved by out2 : %lu\n\r",pwm2);

pwm2 = pwm2 * 10; // turn 200 into 2000 for us, min should be 100
//printf("messages recieved by out1 : %lu\n\r",pwm2);

// the funciton rtos_msg_send, sends the value given as the
// second parameter to the function given as the first
}

//second pwm pin b4 pin 40 on pic
output_bit(PIN_B4,1);
delay_us(pwm2);
output_bit(PIN_B4,0);

//task debud info on lcd
////////////////////////
if(task4_toggle == 0)
{
lcd_gotoxy( 10,2);
printf(lcd_putc,"out2 >> ");
task4_toggle = 1;
}
else
{
lcd_gotoxy( 10,2);
printf(lcd_putc,"out2 >>>");
task4_toggle = 0;
}
rtos_yield();

}//end of while (1)
}

void state_machine_rtos_task()
{
set_adc_channel(1);
delay_ms(1);
adc_value1 = read_adc();
set_adc_channel(2);
delay_ms(1);
adc_value2 = read_adc();

if(rtos_msg_poll ( )>1){

state_machine_input1 = rtos_msg_read ( );
//state_machine_input2 = rtos_msg_read ( );

// the function rtos_msg_read, reads the first value in the queue
//printf("messages recieved by state_mc : %u\n\r",state_machine_input);
// the funciton rtos_msg_send, sends the value given as the
// second parameter to the function given as the first




//state machine here
if((state == START))
   {
   //startup outputs
   adc_value1 = 140;
   adc_value2 = 160;
   
   rtos_msg_send(output2_rtos_task,adc_value1);
   rtos_msg_send(output1_rtos_task,adc_value2);
   
   lcd_gotoxy( 1,4);
   printf(lcd_putc,"RADAR");
   
   state = RADAR;
   }

else if ((state == RADAR)&& state_machine_input1 == BOX)
   {
   //outputs
   adc_value1 = 100;
   adc_value2 = 100;
   
   rtos_msg_send(output2_rtos_task,adc_value1);
   rtos_msg_send(output1_rtos_task,adc_value2);
   
   lcd_putc('\f');
   lcd_gotoxy( 1,4);
   printf(lcd_putc,"CHASE");
   
   state = CHASE;

}
else if((state == CHASE)&& (state_machine_input1 == BOX_LEFT))
   {
   //outputs
   adc_value1 = 140;
   adc_value2 = 160;
   
   rtos_msg_send(output2_rtos_task,adc_value1);
   rtos_msg_send(output1_rtos_task,adc_value2);
   
   lcd_putc('\f');
   lcd_gotoxy( 1,4);
   printf(lcd_putc,"CHASE LEFT");
   
   state = CHASE_LEFT;
   }
   
else if((state == CHASE)&& (state_machine_input1 == BOX_RIGHT))
   {
   //outputs
   adc_value1 = 160;
   adc_value2 = 140;
   
   rtos_msg_send(output2_rtos_task,adc_value1);
   rtos_msg_send(output1_rtos_task,adc_value2);
   
   lcd_putc('\f');
   lcd_gotoxy( 1,4);
   printf(lcd_putc,"CHASE RIGHT");
   
   state = CHASE_RIGHT;
   }
   
else if((state == CHASE_LEFT)&& (state_machine_input1 == BOX))
   {
   //outputs
   adc_value1 = 100;
   adc_value2 = 100;
   
   rtos_msg_send(output2_rtos_task,adc_value1);
   rtos_msg_send(output1_rtos_task,adc_value2);
   
   lcd_putc('\f');
   lcd_gotoxy( 1,4);
   printf(lcd_putc,"CHASE (L)");
   
   state = CHASE;
   }
   
else if((state == CHASE_RIGHT)&& (state_machine_input1 == BOX))
   {
   //outputs
   adc_value1 = 100;
   adc_value2 = 100;
   
   rtos_msg_send(output2_rtos_task,adc_value1);
   rtos_msg_send(output1_rtos_task,adc_value2);
   
   lcd_putc('\f');
   lcd_gotoxy( 1,4);
   printf(lcd_putc,"CHASE (R)");
   
   state = CHASE;
   }
else if((state == CHASE_LEFT)&& (state_machine_input1 == BOX_GONE))
   {
   //outputs
   adc_value1 = 140;
   adc_value2 = 160;
   
   rtos_msg_send(output2_rtos_task,adc_value1);
   rtos_msg_send(output1_rtos_task,adc_value2);
   
   lcd_putc('\f');
   lcd_gotoxy( 1,4);
   printf(lcd_putc,"RADAR (L)");
   
   state = RADAR;
   }
else if((state == CHASE_RIGHT)&& (state_machine_input1 == BOX_GONE))
   {
   //outputs
   adc_value1 = 160;
   adc_value2 = 140;
   
   rtos_msg_send(output2_rtos_task,adc_value1);
   rtos_msg_send(output1_rtos_task,adc_value2);
   
   lcd_putc('\f');
   lcd_gotoxy( 1,4);
   printf(lcd_putc,"RADAR (R)");
   
   state = RADAR;
   }
else if((state == CHASE)&& (state_machine_input1 == BOX_GONE))
   {
   //outputs
   adc_value1 = 160;
   adc_value2 = 140;
   
   rtos_msg_send(output2_rtos_task,adc_value1);
   rtos_msg_send(output1_rtos_task,adc_value2);
   
   lcd_putc('\f');
   lcd_gotoxy( 1,4);
   printf(lcd_putc,"RADAR (CHASE)");
   
   state = RADAR;
   }
else if((state == CHASE_LEFT)&& (state_machine_input1 == BOX_GONE))
   {
   //outputs
   adc_value1 = 160;
   adc_value2 = 140;
   
   rtos_msg_send(output2_rtos_task,adc_value1);
   rtos_msg_send(output1_rtos_task,adc_value2);
   
   lcd_putc('\f');
   lcd_gotoxy( 1,4);
   printf(lcd_putc,"RADAR (L)");
   
   state = RADAR;
   }
 }
}
   
//!//task debud info on lcd
//!if(task5_toggle == 0){
//!{
//!lcd_gotoxy( 1,3);
//!printf(lcd_putc,"state_mc >> ");
//!task5_toggle = 1;
//!}
//!else
//!{
//!lcd_gotoxy( 1,3);
//!printf(lcd_putc,"state_mc >>> ");
//!task5_toggle = 0;
//!}
//!}

void main ( ) {

// initialize input variables
index=0;
input_ready=FALSE;

//initialise portb to be outputs
SET_TRIS_B( 0x00 );

//initialse pin a4 to be an input
SET_TRIS_a( 0xdf ); // 11011111

//initialise pin a0 as analog input and select ad channel 0
setup_adc(ADC_CLOCK_INTERNAL );
setup_adc_ports( AN0 );
set_adc_channel(0);

// initialize interrupts
enable_interrupts(int_rda);
enable_interrupts(global);


//init lcd
lcd_init();
lcd_putc('\f');
printf ( "starting..rtos" );
lcd_gotoxy( 1,4);
printf (lcd_putc, "starting..rtos" );
delay_ms(1000);
lcd_putc('\f');


rtos_run();
}