File content as of revision 29:1132155bc7da:

#include "mbed.h"
#include "dbgprint.h"
#include "robot.h"
#include "hcsr04.h"

BusOut leds(LED_RED,LED_GREEN,LED_BLUE);
Serial pc(USBTX, USBRX);

robot bot;
servo s(PTC9);

/*
//actual competition code, kinda
int firstSection(){  // drive forward and measure distance
    tmpOut = bot.absDriveForward(targetAngle,3000);
    // ping sensor
    tmpOut = bot.absDriveForward(targetAngle,3000);
    // ping sensor
}
int secondsection(){
    // second step
    // etc
}
*/

int main() {
    // Initialize variables
    leds = 0x2;
    char tmpchar = 0;
    const int* constbuf;  // displays values
    int tmpOut;
    double targetAngle=0.0;
    DBGPRINT("AA\n\r",1);  // to see if robot powers up at least
    
    //test servo
    s.toPosition(45);
    
    
    //timer issue navigation test
    bot.absDriveForward(0,50);//assuming the bot stops in this function
    /*
    DBGPRINT("%f",bot.gyro.getZDegrees());
    bot.gyro.stop();//stopping the gyro timer
    //bot.gyro.gyroUpkeepTicker.detach();//stopping the gyro timer
    //use sensor to find distance
    bot.pingLeft.trigger();
    wait(0.1);
    float pingresult = bot.pingLeft.inches();
    DBGPRINT("Distance = %f\r\n",pingresult);
    bot.gyro.start();//stopping the gyro timer
    DBGPRINT("%f",bot.gyro.getZDegrees());
    */
    Timer dataTimer;
    int testdata[]={'H','e','l','l','o'};
    int responseData[17];
    int response;
    int gotAck=0;
    // Loop
    while(1) {
        DBGPRINT("BB\n\r",1);
        leds = leds^0x7; // toggle the LEDs for each loop
        tmpchar = pc.getc();
        bot.BTLink.procBuf(0x02);
        // all of these movement commands are blocking, so they can't be easily stopped short of resetting the microcontroller
        switch(tmpchar){
            /*case 'e': //drive in a smallish square
                bot.driveForward(0,3000);
                bot.driveForward(-90,0);
                bot.driveForward(-90,3000);
                bot.driveForward(-180,0);
                bot.driveForward(-180,3000);
                bot.driveForward(-270,0);
                bot.driveForward(-270,3000);
                bot.driveForward(0,0);
                DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                break;*/
            case 'q': //poll the encoders
                constbuf = bot.bigenc.getVals();
                DBGPRINT("\n\r%d\t%d\t%d\t%d\n\r",constbuf[0],constbuf[1],constbuf[2],constbuf[3]);
                break;
            case 'z': //set the current direction to "forward" for the following "go forward/reverse" commands
                targetAngle = bot.gyro.getZDegrees();
                break;
            case 'w': // turn 90 degrees counter clockwise from the starting rotation
                
                s.toPosition(0);
                //bot.driveForward(90,0);
                DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                break;
            case 'x': // turn 90 degrees clockwise from the starting rotation
                s.toPosition(180);
                //bot.driveForward(-90,0);
                DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                break;
            case 'a': // turn the opposite direction from the starting one
                s.toPosition(90);
                //bot.driveForward(180,0);
                DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                break;
            case 'd': // turn back to starting rotation
                //bot.driveForward(0,0);
                response=bot.BTLink.sendCmd(0x02,testdata,5);
                DBGPRINT("=%d\n\r",response);
                break;
            case 'W': // drive forward a small bit
                //bot.driveForward(targetAngle,1000);
                //bot.motors.moveForward(100);
                tmpOut = bot.absDriveForward(targetAngle,3000);
                DBGPRINT("W -> %d\n\r",tmpOut);
                DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                break;
            case 'E': //drive forward five times as much
                //bot.driveForward(targetAngle,5000);
                bot.absDriveForward(0,5000);
                DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                break;
            case 'X': // small reverse
                //bot.driveForward(targetAngle,-1000);
                
                tmpOut = bot.absDriveForward(targetAngle,-3000);
                DBGPRINT("X -> %d\n\r",tmpOut);
                DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                break;
            case 'C': // big reverse
                //bot.driveForward(targetAngle,-5000);
                bot.absDriveForward(0,-5000);
                DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                break;
            case 'p': // poll the gyro
                DBGPRINT("%d\r\n",bot.gyro.getZ());
                break;
            case 'l': //calibrate the gyro ( caution, only use after resetting and before moving, otherwise will break any calibration)
                bot.gyro.calibrate();
                break;
                
                // these are all control system modifications which should be fairly well locked down at this point
            case 't':
                response=bot.BTLink.sendCmd(0x00,testdata,5);
                //bot.gyro.stop();
                dataTimer.start();
                dataTimer.reset();
                while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
                dataTimer.stop();
                //bot.gyro.start();
                //bot.pfac*=2;
                //DBGPRINT("pfac = %f\r\n",bot.pfac);
                DBGPRINT("Mode0=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
                break;
            case 'g':
                DBGPRINT("full rotation %d,%f -> ",bot.gyro.getZ(),bot.gyro.getZDegrees());
                bot.smoothMove(8000, 1, 40);
                DBGPRINT(" %d,%f\r\n",bot.gyro.getZ(),bot.gyro.getZDegrees());
                break;
            case 'y':
                bot.pollForShapes();
                break;
            case 'h':
                DBGPRINT("full rotation %d,%f -> ",bot.gyro.getZ(),bot.gyro.getZDegrees());
                bot.smoothMove(-8000, 1, 40);
                DBGPRINT(" %d,%f\r\n",bot.gyro.getZ(),bot.gyro.getZDegrees());
                break;
            case 'u':
                bot.pollForRigs();
                break;
            case 'j':
                //bot.dfac/=2;
                //DBGPRINT("dfac = %f\r\n",bot.dfac);
                break;
            case 'i':
                //put distance ping here
                bot.pingLeft.trigger();
                wait(0.1);
                float pingresult = bot.pingLeft.inches();
                DBGPRINT("Distance = %f\r\n",pingresult);
                break;
            case 'k':
                //bot.angfac/=2;
                //DBGPRINT("angfac = %f\r\n",bot.angfac);
                break;
            case 'T':
                //bot.pfac*=1.05;
                //DBGPRINT("pfac = %f\r\n",bot.pfac);
                break;
            case 'G':
                //bot.pfac/=1.05;
                //DBGPRINT("pfac = %f\r\n",bot.pfac);
                break;
            case 'Y':
                //bot.ifac*=1.05;
                //DBGPRINT("ifac = %f\r\n",bot.ifac);
                break;
            case 'H':
                //bot.ifac/=1.05;
                //DBGPRINT("ifac = %f\r\n",bot.ifac);
                break;
            case 'U':
                //bot.dfac*=1.05;
                //DBGPRINT("dfac = %f\r\n",bot.dfac);
                break;
            case 'J':
                //bot.dfac/=1.05;
                //DBGPRINT("dfac = %f\r\n",bot.dfac);
                break;
                
                // poll the compass ( currently gives bad values )
            case 'Q':
                //bot.gyro.checkCompass();
                DBGPRINT("Compass vector = %d\t%d\t%d\t%f\r\n",bot.gyro.xmag,bot.gyro.ymag,bot.gyro.zmag,bot.gyro.compDir*180.0/3.14159);
                break;
                
               // brake if we get an unknown command
            default:
                //bot.left.brake();
                //bot.right.brake();
                break;
                
        }
    }
}