File content as of revision 13:57ea4e520dbd:

#include "motion.h"
#include "geometryfuncs.h"


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.stop();
    Thread::wait(1500);
    ai.thr_AI.signal_set(0x01);

    //PID declare
    PID PIDControllerMotorTheta2(FWD_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f);     //Going forward
    PID PIDControllerMotorTheta(SPIN_MOVE_P, 0, 0, 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;

    while (1) {
        if (ai.flag_terminate) {
            terminate();
        }

        // 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();

        /*
        //PID Tuning Code
        if (pc.readable() == 1) {
            float cmd;
            pc.scanf("%f", &cmd);
            //Tune PID referece
            PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
        }
        */

        // check if target reached ----------------------------------
        if (  ( abs(currX - ai.target.x) < POSITION_TOR )
                &&( abs(currY - ai.target.y) < POSITION_TOR )
           ) {

            diffDir = rectifyAng(currTheta - ai.target.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) {

                static unsigned int lasttargetnumber = 0;
                if (ai.targetnumber > lasttargetnumber) {
                    ai.thr_AI.signal_set(0x01);
                    lasttargetnumber++;
                }
            }
        }

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

            // calc direction to target
            float targetDir = atan2(ai.target.y - currY, ai.target.x - currX);
            if (!ai.target.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) {   //roughly 32 degrees
                //ANGLE_TOR*4
                float tempPidVar = PIDControllerMotorTheta.compute();
                motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
                //pc.printf("spin,%f\n",diffDir);

            } else {

                float tempPidVar = PIDControllerMotorTheta2.compute();
                //pc.printf("turn,%f\n",diffDir);
                // calculte the motor speeds
                if (tempPidVar >= 0) {
                    //turn left
                    speedL = (1-abs(tempPidVar))*MOVE_SPEED;
                    speedR = MOVE_SPEED;

                } else {
                    //turn right
                    speedR = (1-abs(tempPidVar))*MOVE_SPEED;
                    speedL = MOVE_SPEED;
                }
                if (ai.target.facing) motors.setSpeed( int(speedL),  int(speedR));
                else motors.setSpeed( -int(speedR),  -int(speedL));

            }
        }

        // wait
        Thread::wait(MOTION_UPDATE_PERIOD);
    }
}