File content as of revision 24:7a3906c2f5d5:

#include "motion.h"
#include "geometryfuncs.h"
#include "system.h"
#include "PID.h"

AnalogIn ObsAvoidPin(p20);

Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
        thr_motion(mtwrapper,this,osPriorityNormal,1024),
        motors(motorsin),
        ai(aiin),
        kalman(kalmanin) { }

// motion control thread ------------------------
void Motion::motion_thread() {
    motors.resetEncoders();
    motors.setSpeed(5,5);
    motors.stop();
    // Thread::wait(1500);
    //ai.thr_AI.signal_set(0x01);

    //PID declare
    PID PIDControllerMotorTheta2(FWD_MOVE_P, FWD_MOVE_P/10.0f, 0.000005, MOTION_UPDATE_PERIOD/1000.0f);     //Going forward
    PID PIDControllerMotorTheta(SPIN_MOVE_P, SPIN_MOVE_P/10.0f, 0.000005, MOTION_UPDATE_PERIOD/1000.0f);    //Spinning on the spot

    //PID Initialisation
    PIDControllerMotorTheta2.setMode(MANUAL_MODE);
    PIDControllerMotorTheta.setMode(MANUAL_MODE);

    PIDControllerMotorTheta2.setBias(0);
    PIDControllerMotorTheta.setBias(0);

    PIDControllerMotorTheta2.setOutputLimits(-1, 1);
    PIDControllerMotorTheta.setOutputLimits(-1, 1);

    PIDControllerMotorTheta2.setInputLimits(-PI, PI);
    PIDControllerMotorTheta.setInputLimits(-PI, PI);

    PIDControllerMotorTheta.setSetPoint(0);
    PIDControllerMotorTheta2.setSetPoint(0);

    float currX, currY,currTheta;
    float speedL,speedR;
    float diffDir;
    float xBuffer, yBuffer;
    float xOriginalBuffer = 0, yOriginalBuffer = 0;
    int initiateFlag = 1;
    int dontSpinFlag = 0;
    int atTargetFlag = 0;
    int atTargetDirectionFlag = 0;

    while (1) {
        //kalman.statelock.lock();
        if (ai.flag_terminate) {
            // stops motors and teminates the thread
            motors.coastStop();
            terminate();
        }

        // stops motor
        if ((ai.flag_motorStop) || (ObsAvoidPin > 0.5)) {
            motors.stop();
        } else if (ai.flag_manOverride) {

        } else {


            // get kalman localization estimate ------------------------
            kalman.statelock.lock();
            currX = kalman.X(0)*1000.0f;
            currY = kalman.X(1)*1000.0f;
            currTheta = kalman.X(2);
            kalman.statelock.unlock();

            // make a local copy of the target
            ai.targetlock.lock();
            AI::Target loctarget = ai.gettarget();
            ai.targetlock.unlock();
            
            /*
            //PID Tuning Code
            if (pc.readable() == 1) {
                float cmd;
                pc.scanf("%f", &cmd);
                //Tune PID referece
                PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
            }
            */


            if (initiateFlag == 1) {
                xOriginalBuffer = currX;
                yOriginalBuffer = currY;

                xBuffer = ai.gettarget().x;
                yBuffer = ai.gettarget().y;

                initiateFlag = 0;
            }

            if (xBuffer != loctarget.x || yBuffer != loctarget.y) {
                //target changed
                //update xOriginal and yOriginal buffers
                xOriginalBuffer = currX;
                yOriginalBuffer = currY;

                xBuffer = loctarget.x;
                yBuffer = loctarget.y;

                atTargetFlag = 0;
                atTargetDirectionFlag = 0;

            }

            // check if target reached ----------------------------------
            if (atTargetFlag || hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {

                if (atTargetFlag == 0) {
                    motors.stop();
                    Thread::wait(100);
                }


                if (hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
                    atTargetFlag = 1;
                }
                OLED4 = 1;

                diffDir = rectifyAng(currTheta - loctarget.theta);
                //diffSpeed = diffDir / PI;

                PIDControllerMotorTheta.setProcessValue(diffDir);
                float tempPidVar = PIDControllerMotorTheta.compute();
                motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));

                if (abs(diffDir) < ANGLE_TOR) {

                    if (atTargetDirectionFlag == 0) {
                        ai.thr_AI.signal_set(0x01);
                        atTargetDirectionFlag = 1;
                    }

                    /*
                    if (!loctarget.reached) {
                        static int counter = 10;
                        // guarding counter for reaching target
                        if (counter-- == 0) {
                            counter = 10;
                            ai.target.reached = true;
                            ai.thr_AI.signal_set(0x01);

                        }
                    }
                    */
                }
            }

            // adjust motion to reach target ----------------------------
            else {

                OLED3 = 1;

                /*
                if ((hypot(xOriginalBuffer - loctarget.x, yOriginalBuffer - loctarget.y) - hypot(xOriginalBuffer - currX, yOriginalBuffer - currY)) < 0) {
                    loctarget.facing = !loctarget.facing;
                    dontSpinFlag = 1;
                } else {
                    dontSpinFlag = 0;
                }
                */

                // calc direction to target
                float targetDir = atan2(loctarget.y - currY, loctarget.x - currX);
                if (!loctarget.facing) targetDir =  targetDir + PI;

                //Angle differene in -PI to PI
                diffDir = rectifyAng(currTheta - targetDir);

                //Set PID process variable
                PIDControllerMotorTheta.setProcessValue(diffDir);
                PIDControllerMotorTheta2.setProcessValue(diffDir);

                //if diffDIr is neg, spin right
                //if diffDir is pos, spin left

                if ((abs(diffDir) > ANGLE_TOR*4) && (dontSpinFlag == 0)) {   //roughly 32 degrees
                    //ANGLE_TOR*4
                    float tempPidVar = PIDControllerMotorTheta.compute();
                    motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
                    //pc.printf("m1Cmd,%f\n",diffDir);

                } else {
                    

                    float tempPidVar = PIDControllerMotorTheta2.compute();
                    float MoveSpeedLimiter = 1;
                    //pc.printf("m1Speed,%f\n",diffDir);

                    float distanceToX = (float)abs(currX - loctarget.x);
                    float distanceToY = (float)abs(currY - loctarget.y);

                    float distanceToTarget = hypot(distanceToX, distanceToY);

                    if ((distanceToTarget < 400) && (distanceToTarget > 200)) {
                        MoveSpeedLimiter = (distanceToTarget)/400;
                    } else if (distanceToTarget <= 200) {
                        MoveSpeedLimiter  = 0.5;
                    }




                    // calculte the motor speeds
                    if (tempPidVar >= 0) {
                        //turn left
                        speedL = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
                        speedR = MOVE_SPEED*MoveSpeedLimiter;

                    } else {
                        //turn right
                        speedR = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
                        speedL = MOVE_SPEED*MoveSpeedLimiter;
                    }




                    if (loctarget.facing) motors.setSpeed( int(speedL),  int(speedR));
                    else motors.setSpeed( -int(speedR),  -int(speedL));
                    
                }
            }
        }
        //kalman.statelock.unlock();
        // wait
        Thread::wait(MOTION_UPDATE_PERIOD);
    }
}