drive.c

Go to the documentation of this file.

#include "main.h"

short speed = 150; //linear speed
short turnSpeed = 150; //speed to turn

float turnScalar = 5; //use this to calibrate turn-ness per robot
float distScalar = 2.5; //use this to calibrate linear distance - bigger number = shorter dist


char sumData[3]; //temp var, used for debug

float sum = 0;
short ret = 0; //return value, includes distance traveled, bump detect, and groundData
char bumpL = 0; //left bump data
char bumpR = 0; //right bump data


void turn_right(oi_t *sensor, int degrees) { //turns clockwise degrees degrees
    sum = 0;
    oi_update(sensor);
    oi_setWheels(-turnSpeed, turnSpeed);
    while (sum < degrees) {
        oi_update(sensor);
        sum += -turnScalar * (float) sensor->angle;
        timer_waitMillis(20); //wait for sensors to update
    }
    stopmoving(); // stop
}


void turn_left(oi_t *sensor, int degrees) { //turns anticlockwise
    sum = 0;
    oi_update(sensor);
    oi_setWheels(turnSpeed, -turnSpeed);
    while (sum < degrees) {
        oi_update(sensor);
        sum += turnScalar * (float) sensor->angle;
        timer_waitMillis(20); //wait for sensors to update
    }
    stopmoving(); // stop

}


short move_back(oi_t *sensor, short cm) {
    short sumBack = 0;
    oi_update(sensor);
    oi_setWheels(-speed, -speed);
    while (abs(sumBack) < cm*10) {
        oi_update(sensor);
        sumBack += distScalar * (float)sensor->distance; //measure traveled distance, scale to fit
    }
    stopmoving();
    return sumBack;
}


short move_forward(oi_t *sensor, short cm) { //moves forward cm
    sum = 0;
    ret = 0;
    oi_update(sensor);
    oi_setWheels(speed, speed);

    while ((sum) < cm*10) {

        sum += distScalar * (float)sensor->distance; //measure traveled distance, scale to fitt

        oi_update(sensor);
        bumpL = sensor->bumpLeft; //get bumper data
        bumpR = sensor->bumpRight;

        //detect hole/tape
        char groundDat = getGroundData(sensor);
        //if sensor is tripped and bot is moving forward
        if(groundDat) {
            stopmoving(); //stop, don't cross line or hole
            ret += 100*groundDat; //return which sensor was tripped
            break; //end loop
        }


        //detect bump
        //if the bumpers are hit, then stop, backup, turn
        if(bumpL || bumpR) {
            stopmoving();
            //ret += 6; //account for the backup amount (that way we see 1000 or 2000 instead of 995 or 1995
            move_back(sensor, 5); // moves back 5
            timer_waitMillis(200); //wait to ensure bump sensors are clear
            if (bumpL) { //if left (or both) gets hit
                ret += 1000;
                break;
            } else if (bumpR) { // if right gets hit
                ret += 2000;
                break;
            }
        }
    }
    stopmoving();
    return ret + (int)sum/10;
}


void stopmoving() {
    oi_setWheels(0, 0);
}