File content as of revision 32:ff71f61bb9f6:

#include "robot.h"

#define MIN(a,b) ((b<a)?(b):(a))
#define MAX(a,b) ((b>a)?(b):(a))

//this is the main place pinouts are put
//also sets up the control system constants
robot::robot() : spi(PTD2, PTD3, PTD1), bigenc(spi,PTD0), gyro(PTE0, PTE1),
           /* right(bigenc,0,1,PTA5), left(bigenc,2,3,PTA4), */motors(spi,PTD0), pingLeft(PTA13,PTD5),
           BTLink(PTC4,PTC3) {
    
    //bigenc.setDirections(1,-1,1,1);
    //left.setReversed(1);
    x=y=rot=0;
    
    //set our set point in memory to match the current set point of the encoders
    const int *referenceCounts = bigenc.getReferences();
    motors.setSetPoint(referenceCounts[1],referenceCounts[0]);
    
    //storage for shape positions
    circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
    cameraMode=-1;
    flameLocation=0;
}

//driveforward, but set up so that 
int robot::absDriveForward(double desangle, int distance){
    return smoothMove(distance, 0, 40);
}

int robot::smoothMove(int distance, int rotate, int maxSpeed){
    Timer tim;
    int dir=1;
    // if we are traveling forward a negative distance, go backwards
    if(distance < 0){
        dir = -1; //is multiplied by the final output
        distance = -distance;
    }
    if(rotate)
        rotate=-1;
    else 
        rotate=1;
    int i,move;
    //int maxSpeed=41;
    int stepsPerInc=2;
    int stepsToMax=(maxSpeed-1)*stepsPerInc;
    int distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
    int stopRaise,startFall;
    if(distance<distToMax*2){
        // can't get up to full speed before needing to slow down
        //find new max speed, time to get there, and distance there
        maxSpeed =  ((2*stepsPerInc)+sqrt((double)(2*stepsPerInc)*((2*stepsPerInc)+8*distance)))/(4*stepsPerInc);
        stepsToMax=(maxSpeed-1)*stepsPerInc;
        distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
        stopRaise = stepsToMax;
        startFall = stopRaise+(distance-distToMax*2)/(maxSpeed)+1;
        DBGPRINT("Insufficent Ramp-Up\r\n",1);
    } else {
        // can get up to full speed, so we will
        stopRaise=stepsToMax;
        startFall=stopRaise+(distance-distToMax*2)/maxSpeed+1;
        DBGPRINT("Sufficent Ramp-Up\r\n",1);
    }
    DBGPRINT("[%d %d %d] {%d, %d}\n\r",maxSpeed,stepsToMax,distToMax,stopRaise,startFall);
    tim.start();
    move = 0;
    int totalMove=0;
    gyro.stop();
    int xStorage[40];
    int xMaxVal=-3000;
    int xMinVal=3000;
    long xSumVal=0;
    int zStorage[40];
    int zMaxVal=-3000;
    int zMinVal=3000;
    long zSumVal=0;
    for(i=0;i<startFall+stepsToMax;i++){
        //start clock for this frame
        tim.reset();
        if(i<=stopRaise && i%stepsPerInc==0){
            //increase speed every (stepsPerInc) steps up to maxSpeed
            move++;
        }
        if(i==startFall-1){
            //calibration step before we start falling to get an exact result
            move = distance-totalMove-distToMax;
        }
        if(i>=startFall && (i-startFall)%stepsPerInc==0){
            if(i==startFall) //reset move after calibration step
                move=maxSpeed;
            
            //decrement every stepsPerInc steps until stopped
            move--;
        }
        totalMove+=move;
        //motors.moveForward(move*dir);
        motors.moveWheels(move*dir,move*dir*rotate);
        //DBGPRINT("%d: %d\t%d\r\n",i,move,totalMove);
        while(tim.read_ms()<5);
        gyro.gyroUpkeep();
        
        //compass calibration code, shouldn't be needed
        if(distance==16000 && maxSpeed == 6){
            if(gyro.xmag>xMaxVal)
                xMaxVal=gyro.xmag;
            if(gyro.xmag<xMinVal)
                xMinVal=gyro.xmag;
            if(gyro.zmag>zMaxVal)
                zMaxVal=gyro.zmag;
            if(gyro.zmag<zMinVal)
                zMinVal=gyro.zmag;
            if(i%75==0){
                xSumVal+=gyro.xmag;
                zSumVal+=gyro.zmag;
                xStorage[i/75]=gyro.xmag;
                zStorage[i/75]=gyro.zmag;
                DBGPRINT("Saved Val %d \t%d \t%d \t%f\r\n",i/75,gyro.xmag,gyro.zmag,gyro.getZDegrees());
            }
        }
        addforward(double(move*dir)*0.0035362);
        //waits until 10ms have passed for the 100hz timing
        while(tim.read_ms()<10);
    }
    gyro.start();
    
    //compass calibration code, should never run
    if(distance==16000 && maxSpeed == 6){
        double xAvg = 75.0*(double)xSumVal/(startFall+stepsToMax);
        double zAvg = 75.0*(double)zSumVal/(startFall+stepsToMax);
        double xRMSSum=0;
        double zRMSSum=0;
        for(i=0;i<(startFall+stepsToMax)/75;i++){
            xRMSSum+=(xStorage[i]-xAvg)*(xStorage[i]-xAvg);
            zRMSSum+=(zStorage[i]-zAvg)*(zStorage[i]-zAvg);
        }
        double xRMS = sqrt(10.0*xRMSSum/(startFall+stepsToMax));
        double zRMS = sqrt(10.0*zRMSSum/(startFall+stepsToMax));
        for(i=0;i<(startFall+stepsToMax)/75;i++){
            double compDir = atan2(((double)xStorage[i]-gyro.xoffs)*gyro.xamp,((double)zStorage[i]-gyro.zoffs)*gyro.zamp);
            double compDir2 = atan2(((double)xStorage[i]-xAvg)/xRMS,((double)zStorage[i]-zAvg)/zRMS);
            DBGPRINT("Saved Val %d \t%d \t%d \t%f \t%f\r\n",i,xStorage[i],zStorage[i],compDir*180.0/3.14159,compDir2*180.0/3.14159);
        }
        DBGPRINT("Calibrated to (%d,%f,%f) and (%d,%f,%f)\r\n",xMaxVal-xMinVal,xAvg,xRMS,zMaxVal-zMinVal,zAvg,zRMS);
    }
    return totalMove*dir;
    //tim.stop();//may not need
}

//this is the main thing that both turns and goes forward
// desangle is a angle in degrees to head towards (this is relative to the direction the robot starts pointing in
// distance is a distance to head in that direction in units of encoder ticks
/*int robot::driveForward(double desangle, int distance){
    bigenc.resetAll();
    const int* constbuf = bigenc.getVals();
    int prev[4]={0,0,0,0};
    int distTraveled=0, i;
    double maxPow=0.4;
    int loopcount=0;
    double minmain=0.05;
    double minalt=0.05;
    
    //find a point in front of where we're heading
    int targetang = desangle*4050000.0/360.0;//gyro.getZ();
    int startang = targetang;
    double angle=double(startang)*2*3.14159/4050000.0;
    double targx = x + double((distance==0)?10000:distance)*0.0035362*cos(angle)*1.5;
    double targy = y + double((
    
    ==0)?10000:distance)*0.0035362*sin(angle)*1.5;
    int invfactor = 0;
    
    //if going backwards, point away from the point
    if(distance<0) {
        invfactor = 2025000;
    }
    double realfac=0.1;
    
    int pmain=distance;
    int imain=0;
    int dmain=0;
    
    int dterm=0;
    const int ptol = 75;
    const int dtol = 10;
    while((pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol)|| (realfac <= -0.03 || realfac >= 0.03) //|| (fmod(rot*180.0/3.14159-desangle+3600000.0,360.0) >= 1 && fmod(rot*180.0/3.14159-desangle+3600000.0,360.0) <= 359)){

        //DBGPRINT("=%d of %d [%f] (%d, %d) \t{%f,\t%f,/t%f}",distTraveled,distance, maxPow, constbuf[0], constbuf[1],x,y,rot*180.0/3.14159);
        for(i=0;i<4;i++)
            prev[i]=constbuf[i];
        //wait(0.05);
        constbuf = bigenc.getVals();
        
        //control system (Proportional, Integral, Derivative) = PID controller
        pmain = distance - (constbuf[0]+constbuf[1])/2;
        imain += pmain;
        dmain = ((constbuf[0]+constbuf[1]) - (prev[0]+prev[1]))/2;
        
        //DBGPRINT("%f like %f [%f] {%d,%d,%f,%f}\n\r", gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159))*360.0/4050000.0,angle*180/3.14159,rot,pmain,dmain,fmod(rot*180.0/3.14159-desangle+3600000.0,360.0), realfac);\
        
        //finds the difference between the angle to the imaginary point we're headed to and the current angle and turns it into a power level
        realfac = (gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159)))*angfac;
        realfac = MAX(MIN(realfac,0.3),-0.3); // limits how much the motors can turn to fix the angle
        
        //uses PID control for the forward/back motions and adds in the angular component
        //the forward/back motions is limited to a certain speed
        double leftpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain,maxPow),-maxPow)-realfac;
        double rightpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain,maxPow),-maxPow)+realfac;
        
        //if we haven't settled, but also aren't moving, then speed up until it moves
        if((pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol) || (realfac <= -0.02 || realfac >= 0.02) ){
            if (leftpow>0){
                if (leftpow<minalt){
                    leftpow=minalt;
                }
            }else if (leftpow>-minalt){
                leftpow=-minalt;
            }
            if (rightpow>0){
                if (rightpow<minmain){
                    rightpow=minmain;
                }
            }else if (rightpow>-minmain){
                rightpow=-minmain;
            }
        }
        left.setPower(leftpow);
        right.setPower(rightpow);
        
        //how far we've moved in the last timestep
        int deltaTraveled=(constbuf[0]-prev[0]+constbuf[1]-prev[1])/2;
        //DBGPRINT("\t %d\r\n",deltaTraveled);
        addforward(double(deltaTraveled)*0.0035362); //update our position
        distTraveled+=deltaTraveled;
        loopcount++;
        
        //increase min speed so that it will actually move
        if((dmain<5&&dmain>-5)&&minmain<0.2){
            minmain+=0.003;
        } else if (minmain>=0.05) {
            minmain-=0.003;
        }
        if((dterm<5&&dterm>-5)&&minalt<0.2){
            minalt+=0.003;
        } else if (minalt>=0.05) {
            minalt-=0.003;
        }
    }
    left.brake();
    right.brake();
    DBGPRINT("Loops: %d\r\n",loopcount);
    
    //catch the slowdown movement
    wait(0.2);
    for(i=0;i<4;i++)
        prev[i]=constbuf[i];
    //wait(0.05);
    constbuf = bigenc.getVals();
    addforward(double(constbuf[0]-prev[0]+constbuf[1]-prev[1])*0.0035362/2.0);
    DBGPRINT("loss of %d and %d\n\r",constbuf[0]-prev[0],constbuf[1]-prev[1]);
    return 0;
}*/

//add the motion in the direction that the robot is facing
void robot::addforward(double dist){
    double angle=double(gyro.getZ())*2*3.14159/4050000.0;
    x+=dist*cos(angle);
    y+=dist*sin(angle);
    rot=angle;
}

//doesn't work yet
/*int robot::moveTo(double xInches, double yInches){
    double power=.2;
    turntowards(xInches,yInches);
    double distance=sqrt(pow(xInches-x,2)+pow(yInches-y,2))/0.0035362;
    double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
    double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
    bigenc.resetAll();
    const int* constbuf = bigenc.getVals();
    int prev[4]={0,0,0,0};
    int distTraveled=0, i;
    double maxPow;
    DBGPRINT("going %f at angle %f from current of %f\r\n",distance,angle,currangle);
    while(distTraveled<distance){
        angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
        currangle=double(gyro.getZ())*2*3.14159/4050000.0;
        maxPow=MAX(double(distance-distTraveled-2000)/15000.0,0.1);
        if(currangle>angle+2.0){ //too far to the right, brake left
            left.brake();
        } else {
            left.setPower(MIN(power,maxPow));
        }
        if(currangle<angle-2){
            right.brake();
        } else {
            right.setPo
            
            wer(MIN(power,maxPow));
        }
        DBGPRINT("=%d of %d [%f] (%d, %d, %d, %d) \t{%f,\t%f,\t%f}\r\n",distTraveled,distance, maxPow, constbuf[0], constbuf[1], constbuf[2], constbuf[3],x,y,rot*180.0/3.14159);
        for(i=0;i<4;i++)
            prev[i]=constbuf[i];
        //wait(0.05);
        constbuf = bigenc.getVals();
        int deltaTraveled=MAX((MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1])+MIN(constbuf[2]-prev[2],constbuf[3]-prev[3]))/2,0);
        addforward(double(deltaTraveled)*0.0035362);
        distTraveled+=deltaTraveled;
        //angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
    }
    left.brake();
    right.brake();
    return 0;
    
}*/

//also doesn't work
/*int robot::turntowards(double xInches, double yInches){
    double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
    double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
    double finangle=angle;
    //if(int(currangle-angle)%360>180){ //needs to turn positive degrees
    //    finangle=currangle+double(int(angle-currangle)%360);
    //} else {//negative degrees
    //    finangle=currangle-double(int(currangle-angle)%360);
    //}
    double acc=turn(0.4,finangle);
    if(acc<-0.75 && acc>0.75){
        acc=turn(0.3,finangle);
    }
        
    
    return 1;
}*/
    
//still no
/*double robot::turn(double power, double degrees){
    bigenc.resetAll();
    const int* constbuf = bigenc.getVals();
    int prev[4]={0,0,0,0};
    int startz;
    startz=gyro.getZ();
    int gyroticks=(degrees*4050000)/360;
    double maxPow;
    int nowz=startz;
    int dir=0;
    if(gyroticks>startz){
        right.setPower(-power);
        left.setPower(power);
        dir=1;
    } else {
        right.setPower(power);
        left.setPower(-power);
        dir=-1;
    }
    while((gyroticks-nowz)*dir>0){
        maxPow=MAX(double(abs(gyroticks-nowz))/4050000.0,0.25);
        if(gyroticks<nowz){
            right.setPower(-MIN(power,maxPow));
            left.setPower(MIN(power,maxPow));
        } else {
            right.setPower(MIN(power,maxPow));
            left.setPower(-MIN(power,maxPow));
        }
        DBGPRINT("_%d of %d {%f, %f, %f}\r\n",(gyroticks-nowz)*dir, gyroticks,x,y,rot*180.0/3.14159);
        for(int i=0;i<4;i++)
            prev[i]=constbuf[i];
        constbuf = bigenc.getVals();
        int deltaTraveled;
        if(gyroticks<nowz){
            deltaTraveled=(MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1])-MIN(-constbuf[2]+prev[2],-constbuf[3]+prev[3]))/2;
        } else {
            deltaTraveled=(-MIN(constbuf[0]-prev[0],constbuf[1]-prev[1])+MIN(constbuf[2]-prev[2],constbuf[3]-prev[3]))/2;
        }
        addforward(double(deltaTraveled)*0.0035362);
        nowz=gyro.getZ();
    }
    //right.brake();
    //left.brake();
    return (gyroticks-nowz)*dir;
}*/

int robot::switchCameraMode(int mode){
    if(mode==cameraMode){
        return 1;
    }
    int response=0;
    int gotAck=0;
    int testdata[]={'H','e','l','l','o'};
    stepTimer.start();
    while(1){
        response=BTLink.sendCmd(mode,testdata,5);
        stepTimer.reset();
        while(1){
            gotAck=BTLink.getAck(response);
            if(gotAck || stepTimer.read_ms()>=500)
                break;
        }
        DBGPRINT("Mode%d=%d, %d, %d [%d]\n\r",mode,response,BTLink.bufSize(), gotAck, stepTimer.read_ms());
        if(gotAck)
            break;
    }
    stepTimer.stop();
    cameraMode=mode;
    return 1;
}
int robot::shapeCheck(){
    if(cameraMode!=1){
        switchCameraMode(1);
    }
    int gotAck=0;
    int responseData[16];
    stepTimer.start();
    stepTimer.reset();
    gotAck=0;
    while(1){
        BTLink.procBuf(0x01);
        gotAck=BTLink.getData(0x01, responseData);
        if(gotAck || stepTimer.read_ms()>=1000)
            break;
    }
    stepTimer.stop();
    if(responseData[15]==1&&gotAck){
        for(int i=0;i<16;i++)
            DBGPRINT("%d,",responseData[i]);
        DBGPRINT("\n\r",1);
        circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
        rectX=(responseData[4]<<8)|responseData[3];
        rectY=(responseData[6]<<8)|responseData[5];
        rectRot=responseData[2];
        circleX=(responseData[12]<<8)|responseData[11];
        circleY=(responseData[14]<<8)|responseData[13];
        triX=((responseData[8]&0xF)<<8)|responseData[7];
        triY=((responseData[10]&0xF)<<8)|responseData[9];
        triRot=((responseData[8]&0xF0)>>4)|(responseData[10]&0xF0);
        DBGPRINT("Rect(%d, %d, %d) Tri(%d, %d, %d) Cir(%d, %d)\n\r",rectX,rectY,rectRot,triX,triY,triRot,circleX,circleY);
        return 1;
    } else {
        DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[15],gotAck,stepTimer.read_ms());
        return 0;
    }
}
int robot::pollForShapes(){
    switchCameraMode(1);
    BTLink.clearData();
    int found=0;
    for(int i=0;i<5;i++){
        found=shapeCheck();
        if(found)
            break;
    }
    switchCameraMode(0);
    return found;
}
int robot::rigCheck(){
    if(cameraMode!=2){
        switchCameraMode(2);
    }
    int gotAck=0;
    int responseData[16];
    stepTimer.start();
    stepTimer.reset();
    while(1){
        BTLink.procBuf(0x02);
        gotAck=BTLink.getData(0x02, responseData);
        if(gotAck || stepTimer.read_ms()>=1000)
            break;
    }
    stepTimer.stop();
    if(responseData[3]==1 && gotAck){
        flameLocation = responseData[2];
        DBGPRINT("Found fire at %d in %d\n\r",flameLocation,stepTimer.read_ms());
        return 1;
    } else {
        DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[3],gotAck,stepTimer.read_ms());
        return 0;
    }
}
int robot::pollForRigs(){
    switchCameraMode(2);
    BTLink.clearData();
    int found=0;
    for(int i=0;i<5;i++){
        found=rigCheck();
        if(found)
            break;
    }
    switchCameraMode(0);
    return found;
}