Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

functions.cpp

Committer:
abdsha01
Date:
2015-05-23
Revision:
0:15664f71b21f
Child:
1:bd88d4062c97

File content as of revision 0:15664f71b21f:

// The following file implements all the functions necessary 
// for the robot.
// Please don't modify any of these functions!

#include "mbed.h"
#include "Motor.h"
#include "hcsr04.h"

// Returns value from the QRE1113
// lower numbers mean more refleacive
// more than 3000 means nothing was reflected.
int read_line1()
{
    // Time to record how long input is active
    Timer t;
    
    // Activate the line sensor and then read
    line1.output();
    line1 = 1;
    wait_us(15);
    line1.input();
    
    // Start timer
    t.start();

    // Time how long the input is HIGH, but quit
    // after 1ms as nothing happens after that
    while (line1 == 1 && t.read_us() < 1000);
    return t.read_us();
}

int read_line2()
{
    // Time to record how long input is active
    Timer t;
    
    // Activate the line sensor and then read
    line2.output();
    line2 = 1;
    wait_us(15);
    line2.input();
    
    // Start timer
    t.start();

    // Time how long the input is HIGH, but quit
    // after 1ms as nothing happens after that
    while (line2 == 1 && t.read_us() < 1000);
    return t.read_us();
}

int detect_line()
{
    int line1val = read_line1();
    int line2val = read_line2();
    
    if ((line1val-line2val) > 50) {
        printf("Line detected from front");
        return 1;
    } else if ((line1val-line2val) < -50) {
        printf("Line detected from back");
        return -1;
    } else {
        printf("Line not detected");
        return 0;
    }
}

void reverse()
{
    printf("Reverse\n");
    MotorLeft.speed((chargespeed));
    MotorRight.speed(-(chargespeed));
    wait_ms(1000);
    return;
}

void reverseandturn()
{
    printf("Reverse and turn\n");
    MotorLeft.speed((chargespeed-0.2));
    MotorRight.speed(-(chargespeed-0.1));
    wait_ms(2000);
    return;
}

void charge()
{
    MotorLeft.speed(chargespeed);
    MotorRight.speed(chargespeed);
    return;
}