File content as of revision 24:46eeab7cebc6:

#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':
                bot.pfac/=2;
                DBGPRINT("pfac = %f\r\n",bot.pfac);
                break;
            case 'y':
                response=bot.BTLink.sendCmd(0x01,testdata,5);
                dataTimer.start();
                dataTimer.reset();
                while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
                dataTimer.stop();
                if(0){
                    dataTimer.reset();
                    while(!(gotAck=bot.BTLink.getData(0x02, responseData)) && dataTimer.read_ms()<1000);
                    if(responseData[15]==1){
                        int squareX=(responseData[3]<<8)|responseData[2];
                        int squareY=(responseData[5]<<8)|responseData[4];
                        int squareRot=responseData[1];
                        int circleX=(responseData[11]<<8)|responseData[10];
                        int circleY=(responseData[13]<<8)|responseData[12];
                        int triangleX=(responseData[7]<<8)|responseData[6];
                        int triangleY=(responseData[9]<<8)|responseData[8];
                        int triangleRot=responseData[14];
                        DBGPRINT("Rect(%d, %d, %d) Tri(%d, %d, %d) Cir(%d, %d)",squareX,squareY,squareRot,triangleX,triangleY,triangleRot,circleX,circleY);
                    } else {
                        DBGPRINT("Did not pass hash check",1);
                    }
                }
                //while(!bot.BTLink.getAck(response));
                //bot.ifac*=2;
                //DBGPRINT("ifac = %f\r\n",bot.ifac);
                DBGPRINT("Mode1=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
                break;
            case 'h':
                bot.ifac/=2;
                DBGPRINT("ifac = %f\r\n",bot.ifac);
                break;
            case 'u':
                response=bot.BTLink.sendCmd(0x02,testdata,5);
                dataTimer.start();
                dataTimer.reset();
                while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
                DBGPRINT("Mode2=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
                if(gotAck){
                    dataTimer.reset();
                    while(!bot.BTLink.procBuf(0x02) && !(gotAck=bot.BTLink.getData(0x02, responseData)) && dataTimer.read_ms()<1000);
                    dataTimer.stop();
                    if(responseData[3]==1 && gotAck){
                        DBGPRINT("Found fire at %d in %d\n\r",responseData[2],dataTimer.read_ms());
                    } else {
                        DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[3],gotAck,dataTimer.read_ms());
                    }
                }
                response=bot.BTLink.sendCmd(0x00,testdata,5);
                dataTimer.start();
                dataTimer.reset();
                while(!(gotAck=bot.BTLink.getAck(response)) && dataTimer.read_ms()<500);
                dataTimer.stop();
                DBGPRINT("BackToNormal=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
                bot.BTLink.getData(0x02, responseData);
                //while(!bot.BTLink.getAck(response));
                //bot.dfac*=2;
                //DBGPRINT("dfac = %f\r\n",bot.dfac);
                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':
                DBGPRINT("Compass vector = %d\t%d\t%d\r\n",bot.gyro.xmag,bot.gyro.ymag,bot.gyro.zmag);
                break;
                
               // brake if we get an unknown command
            default:
                //bot.left.brake();
                //bot.right.brake();
                break;
                
        }
    }
}