File content as of revision 49:7d172c133dbf:

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

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



/*
//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() {
    wait(0.1);
    robot bot;
    gamePlayer game(bot);
    /*servo s(PTC9);
    servo s2(PTC8);
    servo s3(PTA5);
    servo s4(PTA4);*/
    // 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);
    s2.toPosition(60);
    s3.toPosition(250);
    s4.toPosition(90);*/
    
    int s1pos = 45;
    int s2pos = 45;
    
    //timer issue navigation test
    bot.absDriveForward(0,50);//assuming the bot stops in this function
    
    bot.gyro.stop();//stopping the gyro timer
    /*
    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':
                DBGPRINT("100 Forward\n\r",1);
                bot.absDriveForward(0,100);
                // run first row
                
                /*DBGPRINT("Running First Row\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                game.resetFirst();
                game.runFirstRow();*/
                break;
            case 'x': // turn 90 degrees clockwise from the starting rotation
                //s.toPosition(220);
                //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);
                
                DBGPRINT("100 Left\n\r",1);
                bot.smoothMove(-100, 1, 40);
                break;
            case 'd': // turn back to starting rotation
                //bot.driveForward(0,0);
                //response=bot.BTLink.sendCmd(0x02,testdata,5);
                //DBGPRINT("=%d\n\r",response);
                DBGPRINT("100 Right\n\r",1);
                bot.smoothMove(100, 1, 40);
                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,290*16);
                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,-290*16);
                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(1){
                    gotAck=bot.BTLink.getAck(response);
                    if(gotAck || dataTimer.read_ms()>=500)
                        break;
                }
                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());
                for(int i=0;i<36;i++){
                    bot.smoothMove((8000.0/360.0)*(10.0), 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':
                DBGPRINT("LeftStable (15,8) = %f\r\n",bot.pingLeft.getStablePoll());
                break;
            case 'i':
                //put distance ping here
                bot.pingLeft.trigger();
                dataTimer.start();
                dataTimer.reset();
                while(bot.pingLeft.measuring && dataTimer.read_ms()<300);
                dataTimer.stop();
                DBGPRINT("Distance = %f\r\n",bot.pingLeft.inches());
                break;
            case 'k':
                dataTimer.start();
                for(int i=0;i<100;i++){
                    bot.pingLeft.trigger();
                    dataTimer.reset();
                    while(bot.pingLeft.measuring && dataTimer.read_ms()<300);
                    DBGPRINT("Distance %d = %f\r\n",i,bot.pingLeft.inches());
                }
                dataTimer.stop();
                //bot.angfac/=2;
                //DBGPRINT("angfac = %f\r\n",bot.angfac);
                break;
            case 'T':
                DBGPRINT("Grabbing Tool\r\n",1);
                //s.toPosition(-45);
                //s2.toPosition(60);
                bot.grabPosition();
                wait(0.5);
                bot.grab();
                /*s2.toPosition(-10);
                wait(0.5);
                //bot.cont.start();
                bot.cont.resetTicks();
                bot.cont.rampSpeed(-20,-20);
                //s.toPosition(0);
                //wait(.5); 
                while(bot.cont.avgTicks()>-900){
                    s.toPosition((-45)+((35+45)*(-bot.cont.avgTicks()))/900);
                    wait_ms(20);
                }
                bot.cont.rampSpeed(0,0);
                s.toPosition(130);
                while(!bot.cont.stopped());*/
                //bot.cont.stop();
                //game.checkWaves();
                break;
            case 'G':
                DBGPRINT("Running First and Second Row\r\n",1);
                game.runFirstRow();
                game.runSecondRow();
                game.runSecondRow();
                game.approachRig();
                //bot.pfac/=1.05;
                //DBGPRINT("pfac = %f\r\n",bot.pfac);
                break;
            case 's':
                DBGPRINT("100 Back\n\r",1);
                bot.smoothMove(-100, 0, 40);
                //game.runSecondRow();    //testing second and third row
                //game.runSecondRow();
                break;
            case 'Y':
                /*bot.cont.start();
                bot.cont.rampSpeed(40,40);
                while(!bot.cont.steady());
                bot.cont.rampSpeed(0,0);
                while(!bot.cont.stopped());
                bot.cont.stop();*/
                //s3.toPosition(s1pos+=5);
                //bot.alignWithRig();
                
                bot.grabPosition();
                DBGPRINT("1 pos = %d\r\n",s1pos);
                break;
            case 'H':
                DBGPRINT("2 pos = %d\r\n",bot.incServo());
                //s3.toPosition(s1pos-=5);
                //DBGPRINT("1 pos = %d\r\n",s1pos);
                break;
            case 'U':
                //s4.toPosition(s2pos+=5);
                bot.extendStick();
                DBGPRINT("2 pos = %d\r\n",s2pos);
                break;
            case 'J':
                //s4.toPosition(s2pos-=5);
                DBGPRINT("2 pos = %d\r\n",bot.decServo());
                //DBGPRINT("2 pos = %d\r\n",s2pos);
                break;
                
                // poll the compass ( currently gives bad values )
            case 'Q':
                bot.downCamera();
                wait(3);
                bot.pollForShapes();
                if(bot.triX != 0){
                    //found triangle
                    float dist = (8.25*(480.0-bot.triY))/480.0 + 1.0;
                    if(!(bot.triRot > 52 && bot.triRot < 67)){
                        bot.extendStick();
                        bot.smoothMove(-1000, 1, 5); // turn left
                        bot.retractStick();
                        bot.smoothMove(50, 1, 5); // turn right
                        if(dist>4.0)
                            bot.smoothMove(290*(dist-5), 0, 5);
                        bot.smoothMove(425, 1, 5); // turn left
                        wait(0.5);
                        bot.pollForShapes();
                    }
                    if(bot.triX != 0 && bot.triRot > 52 && bot.triRot < 67){
                        if(bot.triX > 375 || bot.triX < 360){
                            int correction = (100*(bot.triX - 367))/65;
                            bot.smoothMove(correction, 1, 5); // turn rightish to correct
                            wait(0.5);
                            bot.pollForShapes();
                            
                            //right is positive and decreases
                        }
                        dist = (9.6*(480.0-bot.triY))/480.0 + 0.9;
                        bot.smoothMove(290*(dist-0.2), 0, 5);
                        bot.grab();
                    }
                    
                }
                //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;
                
        }
    }
}