File content as of revision 1:37d0985c814e:

/**
@file obstacle.cpp
@brief Program Implementation
*/

#include "mbed.h"
#include "SRF02.h"
#include "N5110.h"
#include "beep.h"
#include "obstacle.h"


//////////////////////////PART 1 - SETTING UP THE MEASUREMENTS//////////////////////////////

int timerFlag=0;
void timerExpired()
{
    timerFlag =1;
}

void error(int code)
{
    while(1) {
        leds = 0;
        wait(0.25);
        leds = code;
        wait(0.25);
    }
}

void measureExpired()
{
    measureFlag =1;
}


/*
void initTMP102()
{
    tmp102.frequency(400000); // set bus speed to 400 kHz
    int ack; // used to store acknowledgement bit
    char data[2]; // array for data
    char reg = CONFIG_REG; // register address
//////// Read current status of configuration register ///////

    ack = tmp102.write(TMP102_W_ADD,&reg,1); // send the slave write address and the configuration register address
    if (ack)
        error(1); // if we don't receive acknowledgement, flash error message
    ack = tmp102.read(TMP102_R_ADD,data,2); // read default 2 bytes from configuration register and store in buffer
    if (ack)
        error(2); // if we don't receive acknowledgement, flash error message
///////// Configure the register //////////
// set conversion rate to 1 Hz
    data[1] |= (1 << 6); // set bit 6
    data[1] &= ~(1 << 7); // clear bit 7

//////// Send the configured register to the slave ////////////

    ack = tmp102.write(TMP102_W_ADD,&reg,1); // send the slave write address and the configuration register address
    if (ack)
        error(3); // if we don't receive acknowledgement, flash error message
    ack = tmp102.write(TMP102_W_ADD,data,2); // send 2 data bytes to the configuration register
    if (ack)
        error(4); // if we don't receive acknowledgement, flash error message

}

float getTemperature()
{
    int ack; // used to store acknowledgement bit
    char data[2]; // array for data
    char reg = TEMP_REG; // temperature register address
    ack = tmp102.write(TMP102_W_ADD,&reg,1); // send temperature register address
    if (ack)
        error(5); // if we don't receive acknowledgement, flash error message
    ack = tmp102.read(TMP102_R_ADD,data,2); // read 2 bytes from temperature register and store in array
    if (ack)
        error(6); // if we don't receive acknowledgement, flash error message
    temperature = (data[0] << 4) | (data[1] >> 4);
    return temperature*0.0625;
}

float calcSoundSpeed(float temperature)
{
    return soundSpeed = temperature*0.6 + 331.3;
}

float calcSoundTimeDelay (int distance, float soundSpeed)
{   distance =srf.getDistanceCm();
    return soundTimeDelay = distance/soundSpeed;
}
*/


///////////////////PART 2 - Distance Upadate and Speed //////////////////////


void updateExpired()
{
    updateFlag=1;    ///
}

void update_distance_new()
{
    lastDistance = newDistance; ///updates the lastDistance variable by storing into it the previous distance measurement
    newDistance = srf.getDistanceCm();///updates the newDistance variable by adding to it the new distance measurement
}


///////////////////PART 3 - Distance Range Alert//////////////////////////////

void rangeExpired()
{
    rangeFlag =1;
}

struct State {
    float bOutput; /// buzzer output value
    int nextState[5]; /// array of next states
};
typedef const struct State STyp;

STyp fsm[5] = {
    {8000,{0,1,2,3,4}}, // State 0 - object at less than 20 cm away
    {2000,{0,1,2,3,4}}, // 1 - object in the 20-65cm range from the person
    {1000,{0,1,2,3,4}}, // 2 - object in the 65-110cm range from the person
    {500,{0,1,2,3,4}}, // 3 - object in the 110-155cm range from the person
    {200,{0,1,2,3,4}}, // 4 - object in the 155-200cm range from the person

};

void updateState()
{

    distance = srf.getDistanceCm();
    if(distance<=20) rangeState=0;
    else if((distance>20)&&(distance<=65))rangeState=1;
    else if ((distance>65)&&(distance<=110))rangeState=2;
    else if ((distance>110)&&(distance<=155))rangeState=3;
    else if ((distance>155)&&(distance<=200))rangeState=4;


    currentState = fsm[currentState].nextState[rangeState];
}

///////////////////PART 4 - Beam Pattern ///////////////////////////////////

double angleArray[5]= {0,10,20,30,40}; /*!<array of angles from the center of the sensor*/


void beamExpired()
{
    beamFlag=1;
}

double convertDegrees(double angleDegrees)
{
     ///uses equivalence of 2pi and 180 degrees
    return angleDegrees*3.16/180;     
}

void drawSemiCircle()
{
//draws a semi circle on the screen of radius equal to the distance from the object
    int radius = srf.getDistanceCm(); /// int radius = ((30*srf.getDistanceCm())/20); other possibility
    lcd.drawCircle(WIDTH/2,1,radius,0);  /// x,y,radius,transparent with outline
    lcd.drawLine(WIDTH/2,1,WIDTH/2,radius,2); ///draws one diameter
    lcd.refresh();
}

void drawBeamLines()
{
///draws a beam line for each calcualted value
    lcd.drawLine(WIDTH/2,1,WIDTH/2+l,h,2);
    lcd.drawLine(WIDTH/2,1,WIDTH/2-l,h,2);
    lcd.refresh();
}

double lat(double a, double d)
{
    return (tan(a) *d);
}

double hyp(double a, double d)
{
    return ((tan(a) *d)/sin(a));
}

/////////////////////////PART 5 - the Wall Follower Simulation //////////////////////

void wallExpired()
{
    wallFlag=1;
}

void moveForward()
{
/// sets pixel to move forward

    for(int k=0; k<=22; k++) {
        lcd.setPixel(16,k);  /// (x,y) starting point of the pixel - here fixed x coordinate - vertical movement
        wait(0.2);  ///can be considered as the "speed of the movement" because it defines the rate if change of pixel - delay between two k values
        pix++;  ///increments the pix vlaue for each increase in k
        lcd.refresh();
    }
}

void turnLeftForward_one()
{
///sets the pixel to turn left forward

    for(int l=0; l<=64; l++) {
        lcd.setPixel(l+16,22);  /// (x,22) starting point of the pixel - here fixed y coordinate - horizontal movement
        wait(0.2);
        pix=pix+1; ///increments the pix vlaue for each increase in l
        lcd.refresh();
    }

}

void turnLeftForward_two()
{
///sets the pixel to turn right forward -one

    for (int m=0; m<=16; m++) {
        lcd.setPixel(80,m+22);  /// (80,y) starting point of the pixel - here fixed x coordinate - vertical movement
        wait(0.2);
        pix=pix+1; ///increments the pix vlaue for each increase in m
        lcd.refresh();
    }

}

void turnRightForward()
{
///sets the pixel to turn right forward -two

    for(int n=0; n<=78; n++) {
        lcd.setPixel(80-n,38);  /// (x,38) starting point of the pixel - here fixed y coordinate - horizontal movement- x=80-n because counting down from the end x=80 to he beginning x=2 of the screen
        wait(0.2);
        pix=pix+1; ///increments the pix vlaue for each increase in n
        lcd.refresh();
    }
    //lcd.refresh();
}


///intial beep zero move forward
void beep_zero()
{
    int sound_zero = 200; /// local varaible integer gives specific frequency value for the buzzer
    float time_zero = 0.22; /// local varaible integer gives specific time for the buzzer
    buzzer.beep(sound_zero,time_zero);

}

///turns on beep one Turn Left
void beep_one()
{
    int sound_one = 600;
    float time_one = 0.2;
    buzzer.beep(sound_one,time_one);
}

/// turns on beep two Trun Left and move Forward
void beep_two()
{
    int sound_two = 800;
    float time_two = 0.23;
    buzzer.beep(sound_two,time_two);
}

//turns on beep three Turn Right
void beep_three()
{
    int sound_three = 1600;
    float time_three = 0.2;
    buzzer.beep(sound_three,time_three);
}

/// turns on beep four Turn Right and move Forward
void beep_four()
{
    int sound_four = 2000;
    float time_four = 0.14;
    buzzer.beep(sound_four,time_four);
}

/// turns on beep five Stop Dead End
void beep_five()
{
    int sound_five = 2600;
    float time_five = 0.6;
    buzzer.beep(sound_five,time_five);
}

void toggle_red()
{
    red=!red; ///toggle red led
}
void toggle_yellow()
{
    yellow=!yellow;  ///toggles yellow led
}
void toggle_green()
{
    green=!green;  ///toggles green led
}



void move()
{
    if (pix<=22) {
        lcd.printString(" Forward ",32,0);
        beep_ticker_zero.attach(&beep_zero, 0.61); //move forward
        toggle_green_ticker.attach(&toggle_green, 0.61);
        moveForward();
    }

    beep_ticker_zero.detach();
    beep_ticker_one.attach(&beep_one, 0.2);//turn left
    lcd.printString(" T Left ",32,0);
    toggle_green_ticker.detach();
    toggle_red_ticker.attach(&toggle_red,0.1);
    wait(0.5);
    beep_ticker_one.detach();
   

    if (22<pix && pix<86) {
        toggle_red_ticker.detach();
        lcd.printString(" Forward ",32,0);
        beep_ticker_two.attach(&beep_two, 0.66); //turn left and move forward
        toggle_green_ticker.attach(&toggle_green, 0.66);
        turnLeftForward_one();
    }

    beep_ticker_two.detach();
    beep_ticker_three.attach(&beep_three,0.2);//turn right
    lcd.printString(" T Right ",32,0);
    toggle_green_ticker.detach();
    toggle_red_ticker.attach(&toggle_red,0.1);
    wait(0.5);
    beep_ticker_three.detach();
    

    if(86<pix && pix<102) {
        toggle_red_ticker.detach();
        lcd.printString(" Forward ",32,0);
        toggle_green_ticker.attach(&toggle_green, 0.76);
        beep_ticker_four.attach(&beep_four, 0.76);//turn right and move forward
        turnLeftForward_two();
    }

    beep_ticker_four.detach();
    beep_ticker_three.attach(&beep_three,0.2);//turn right
    lcd.printString(" T Right ",32,0);
    toggle_red_ticker.attach(&toggle_red,0.1);
    wait(0.5);
    beep_ticker_three.detach();
   

    if (102<pix) {
        toggle_red_ticker.detach();
        beep_ticker_four.attach(&beep_four,0.8);///turn right and move forward- function called every 0.8s
        toggle_green_ticker.attach(&toggle_green, 0.76); /// toggle green led function called every 0.76s
        lcd.printString(" Forward ",32,0);
        turnRightForward();
    }

    beep_ticker_four.detach();
    toggle_green_ticker.detach();
    lcd.printString("  STOP!! ",32,0);
    toggle_red_ticker.attach(&toggle_red,0.1);
    beep_ticker_five.attach(&beep_five,0.8);//stop dead end
}

void drawMaze()
{
    lcd.drawRect(0,0,12,25,1);  /// top left filled rectangle-green on report
    lcd.drawRect(20,0,10,20,1); /// top middle filled rectangle-orange on report
    lcd.drawRect(30,10,52,10,1); /// top right filled rectangle-pink on report
    lcd.drawRect(0,25,76,10,1); /// middle filled rectangle-purple on report
    lcd.drawRect(0,40,80,1,1); /// bottom filled rectangle-red on report
    lcd.drawRect(0,36,1,4,1); /// dead end small filled rectangle-black on report
    lcd.refresh();
}

///////////////////////////////Transitions/User interface (button) selection and event order elements//////////////////////////////////

void buttonPressed()
{
    buttonFlag++;  ///increments and keepd the value of the buttonFlag counter for every button press
    if ( buttonFlag > 4) {  /// sets the limit value of the buttonFlag before going back to zero => five different flag states to be used for the five different modules
        buttonFlag = 0;
    }
}


void manageScreen()
{
    if ( buttonFlag== 0) {
        measureFlag = !measureFlag; /// toggles measureFlag - if the flag is intially low then it goes high for buttonFlag = 0 - first screen view
    }
    if ( buttonFlag == 1) {
        updateFlag = !updateFlag; /// toggles updateFlag - if the flag is intially low then it goes high for buttonFlag = 1 - first button press - second screen view
    }
    if ( buttonFlag == 2) {

        rangeFlag = !rangeFlag;  /// toggles rangeFlag - if the flag is intially low then it goes high for buttonFlag = 2 - second button press - third screen view
    }
    if ( buttonFlag == 3) {
        beamFlag = !beamFlag;  /// toggles beamFlag - if the flag is intially low then it goes high for buttonFlag = 3 - third button press - fourth screen view
    }
    if ( buttonFlag == 4) {
        wallFlag = !wallFlag;  /// toggles wallFlag - if the flag is intially low then it goes high for buttonFlag = 4 - fourth button press - fifth screen view
    }

    if(measureFlag == 1) {
        measure_ticker.attach(&update_distance_new,1.0);
        //  temperature_ticker.attach(&getTemperature,1.0);
    } else {
        measure_ticker.detach();
    }

    if(updateFlag == 1) {

        update_ticker.attach(&updateExpired,2.0);

    } else {
        update_ticker.detach();     // detach ticker
    }

    if(rangeFlag == 1) {
        range_ticker.attach(&rangeExpired,3.0);
    } else {
        range_ticker.detach();
    }

    if(beamFlag == 1) {
        beam_ticker.attach(&beamExpired,1.0);
    } else {
        beam_ticker.detach();
    }

    if(wallFlag == 1) {
        wall_ticker.attach(&wallExpired, 4.0);
    } else {
        wall_ticker.detach();
    }
}





int main()
{
    lcd.init(); //initializes the LCD display
///  initTMP102();

    lcd.normalMode();      /// normal colour mode
    lcd.setBrightness(0.5); /// put LED backlight on 50%
    button.rise(&buttonPressed); /// indicates that the button has been pressed. button flag rises when button pressed

    measure_ticker.attach(&measureExpired,1.0);
    update_ticker.attach(&updateExpired,2.0);
    beam_ticker.attach(&beamExpired,3.0);
    //  update_ticker_two.attach(&update_distance_new,2.0);
    range_ticker.attach(&rangeExpired,4.0);
    wall_ticker.attach(&wallExpired, 5.0);


    while(1) {
        // distance = srf.getDistanceCm();

        manageScreen();
        if (timerFlag) {
            timerFlag=0;   ///reset flag


            if(measureFlag) {
                lcd.clear();
                measureFlag =0;  ///reset flag

                char buffer[14];

                distance = srf.getDistanceCm();  /// same idea with floats
                float length = sprintf(buffer,"D = %.2f cm",distance);
                if (length <= 14)
                    lcd.printString(buffer,0,4);

            /*    temperature = getTemperature();
                     int lengthT = sprintf(buffer,"T = %.2f C",temperature); /// print formatted data to buffer
                     if (lengthT <= 14)  /// if string will fit on display
                         lcd.printString(buffer,0,5);
               
                soundSpeed = calcSoundSpeed(temperature);
                soundTimeDelay = calcSoundTimeDelay(distance, soundSpeed);
                */
            }
            lcd.refresh();


            if(updateFlag) {
                lcd.clear();
                updateFlag=0;  ///reset flag
                char bufferOne[14]; ///character array that stores the values of the newDistance variable
                char bufferTwo[14];///character array that stores the values of the lastDistance variable



                float lengthN = sprintf(bufferOne,"Dn = %.2f cm",distance);
                if (lengthN <= 14)
                    lcd.printString(bufferTwo,0,4);

                float lengthL = sprintf(bufferTwo,"Dl = %.2f cm",lastDistance);
                if (lengthL <= 14)
                    lcd.printString(bufferTwo,0,5);
            }
            lcd.refresh();


            if (rangeFlag) {
                lcd.clear();
                rangeFlag=0;  ///reset  flag
                char buffer[14];

                //   distance = srf.getDistanceCm();  // same idea with floats
                float length = sprintf(buffer," %.2f cm",distance);
                if (length <= 14) {
                    lcd.printString(buffer,11,2);
                }

                buzzerFrequency=fsm[currentState].bOutput;  /// retrieves the buzzer frequency associated to the currentState from the FSM definition
                buzzer.beep(buzzerFrequency,1);
                float lengthB = sprintf(buffer," %.2f hz",buzzerFrequency);
                if (lengthB <= 14) {
                    lcd.printString(buffer,11,4);
                }
                lcd.refresh();
            }


            if (beamFlag) {
                lcd.clear();
                beamFlag =0;

                z++;
                if (z>=5) z=0;
                angleDegrees = angleArray[z];
                angleRadians=convertDegrees(angleDegrees);
                a=angleRadians;

                drawSemiCircle();
                d =srf.getDistanceCm();
//simply assigns the function results under conditions (a,d) to the distance variable values. calls the pointers defined previously
                lateralDistance = 2*lat(a,d);
                h = hyp(a,d);

                drawBeamLines();

            }
            lcd.refresh();


            if (wallFlag) {
                lcd.clear();
                wallFlag=0; ///reset flag
                drawMaze();  ///draws the screen backgroung structure
                move();    ///controls the movement and associated signals for the moving pixel point
            }

        }
    }
}