File content as of revision 3:463edcfc09c5:

#include "mbed.h"
#include "iC_MU.h"
#include "rtos.h"
#include "MyAxis.h"

extern iC_MU pan_Motor, PanPos;
extern PwmOut Pan_Motor_PWM;

extern iC_MU tilt_Motor, TiltPos;
extern PwmOut Tilt_Motor_PWM;

extern DigitalOut Pan_Motor_Direction;
extern DigitalOut Tilt_Motor_Direction;

extern Serial pc;

extern float Pan_count;

// Deal with profiling and scoping (generic)
bool scoping;
int scope_duration = 1000;
int scope_time = 0;
float amplitude;
float a;
float Hz;
extern float start_Hz;
extern float stop_Hz;
extern float run_time;
float jogaccel_time = 2000;
float jogaccel = 1;
float dwell = 1000;
float bits = 19;
int OneTurnCts = pow(2, bits); //491520
int MyLower = pow(2,(bits-4));
int MyUpper = OneTurnCts - MyLower;
int skip = 0;
int primed = 0;


// Motor Generics
float c;
float m;
float Duty_Cycle = 0;
float Motor_Direction;
float Motor_PWM;
float DeadBand = 0.012;
int Motor_wrap = 4194304;


void PanAxisInitialise()
{
    //MyAxis Pan;
    Pan.Number = 1;
    Pan.jog = 3;
    Pan.sysjogdir = 1;
    Pan.sinusoidrun = 0;
    Pan.scoping = 0;
    Pan.setramp = 0;
    Pan.setramp_x = 0;
    Pan.moveme = 0;
    Pan.ramp = 0.05;
    Pan.run_count = 0;
    Pan.ratio = 115;  //This is 115 on original breadboard, -113 on a prototype MS5
    Pan.Motor_scale = 11100; //15728;  //(based on 1800RPM, 2^19 encoder and 0.001s loop)
    Pan.c_moving = 20;
    Pan.c_stationary = 2;
    Pan.Jog_1 = 0;
    Pan.Jog_2 = 60;
    Pan.Kp = 0.060;
    Pan.Kp_Sys = 0.000;
    Pan.Kff = 1;
    Pan.Kff_Sys = 1;
    Pan.Feed_Forward = 0;
    Pan.Prev_Motor_final = 0;
    Pan.Motor_Vel = 0;
    Pan.Motor_position = 0;
    Pan.dojog = 0;
    Pan.sysdojog = 0;
    Pan.jogdir = 1;
    Pan.sysjogdir = 1;
    Pan.start_MotPos = 0;


    // Set up the Pan motor
    Pan_Motor_PWM.period_us(50);                // Set PWM to 20 kHz
    Pan_Motor_PWM = 1;                          // Start with motor static
    Pan.Motor_Inst_Pos = pan_Motor.ReadPOSITION();
    Pan.MotorLast_Inst_Pos = Pan.Motor_Inst_Pos;

    Pan.Motor_position = 0;
    Pan.MotorDemandPos = 0;
    Pan.System_position = PanPos.ReadPOSITION();
    Pan.SysDemandPos = Pan.System_position;
    Pan.SysDemand_Prev = Pan.SysDemandPos;
    Pan.Sys_follow_error = Pan.SysDemandPos - Pan.System_position;
    //pc.printf("\n\r %d,%f,%f,%f,%f",scope_time,Axis.Motor_position,Axis.System_position,Axis.SysDemandPos,Axis.Sys_follow_error);
}

void TiltAxisInitialise()
{
    //MyAxis Pan;
    Tilt.Number = 2;
    Tilt.jog = 3;
    Tilt.sysjogdir = 1;
    Tilt.sinusoidrun = 0;
    Tilt.scoping = 0;
    Tilt.setramp = 0;
    Tilt.setramp_x = 0;
    Tilt.moveme = 0;
    Tilt.ramp = 0.05;
    Tilt.run_count = 0;
    Tilt.ratio = -115;  // This is -115 on original prototype unit, -113 on a prototype MS5)
    Tilt.Motor_scale = 11100; //15728;  //(based on 1800RPM, 2^19 encoder and 0.001s loop)
    Tilt.c_moving = 20;
    Tilt.c_stationary = 2;
    Tilt.Jog_1 = 0;
    Tilt.Jog_2 = 60;
    Tilt.Kp = 0.06;
    Tilt.Kp_Sys = 0.0005;
    Tilt.Kff = 1;
    Tilt.Kff_Sys = 1;
    Tilt.Feed_Forward = 0;
    Tilt.Prev_Motor_final = 0;
    Tilt.Motor_Vel = 0;
    Tilt.Motor_position = 0;
    Tilt.dojog = 0;
    Tilt.sysdojog = 0;
    Tilt.jogdir = 1;
    Tilt.sysjogdir = 1;
    Tilt.start_MotPos = 0;


    // Set up the Tilt motor
    Tilt_Motor_PWM.period_us(50);                // Set PWM to 20 kHz
    Tilt_Motor_PWM = 1;                          // Start with motor static
    Tilt.Motor_Inst_Pos = tilt_Motor.ReadPOSITION();
    Tilt.MotorLast_Inst_Pos = Tilt.Motor_Inst_Pos;

    Tilt.Motor_position = 0;
    Tilt.MotorDemandPos = 0;
    Tilt.System_position = TiltPos.ReadPOSITION();
    Tilt.SysDemandPos = Tilt.System_position;
    Tilt.SysDemand_Prev = Tilt.SysDemandPos;
    Tilt.Sys_follow_error = Tilt.SysDemandPos - Tilt.System_position;
    //pc.printf("\n\r %d,%f,%f,%f,%f",scope_time,Axis.Motor_position,Axis.System_position,Axis.SysDemandPos,Axis.Sys_follow_error);
}

void UpdatePanFeedback()
{
    Pan.MotorLast_Inst_Pos = Pan.Motor_Inst_Pos;
    Pan.Motor_Inst_Pos = pan_Motor.ReadPOSITION();

    Pan.Motor_Vel =  (Pan.Motor_Inst_Pos - Pan.MotorLast_Inst_Pos);

    // Check to see if we have gone past the index point
    if( (Pan.Motor_Inst_Pos < MyLower) && (Pan.MotorLast_Inst_Pos > MyUpper) ) {            // We have gone over the index point in 1 direction
        Pan.Motor_Vel += OneTurnCts;
    } else if( (Pan.Motor_Inst_Pos > MyUpper) && (Pan.MotorLast_Inst_Pos < MyLower) ) {     // We have gone over the index point in the other direction
        Pan.Motor_Vel -= OneTurnCts;
    }

    Pan.Motor_position = Pan.Motor_position + Pan.Motor_Vel;

    Pan.System_position_prev = Pan.System_position;
    Pan.System_position = PanPos.ReadPOSITION();
        
    Pan.SysActualVel = Pan.System_position - Pan.System_position_prev;

}
void UpdateTiltFeedback()
{
    Tilt.MotorLast_Inst_Pos = Tilt.Motor_Inst_Pos;
    Tilt.Motor_Inst_Pos = tilt_Motor.ReadPOSITION();

    Tilt.Motor_Vel =  (Tilt.Motor_Inst_Pos - Tilt.MotorLast_Inst_Pos);

    // Check to see if we have gone past the index point
    if( (Tilt.Motor_Inst_Pos < MyLower) && (Tilt.MotorLast_Inst_Pos > MyUpper) ) {            // We have gone over the index point in 1 direction
        Tilt.Motor_Vel += OneTurnCts;
    } else if( (Tilt.Motor_Inst_Pos > MyUpper) && (Tilt.MotorLast_Inst_Pos < MyLower) ) {     // We have gone over the index point in the other direction
        Tilt.Motor_Vel -= OneTurnCts;
    }

    Tilt.Motor_position = Tilt.Motor_position + Tilt.Motor_Vel;
    
  //  Tilt.System_position_prev = Tilt.System_position;
    Tilt.System_position = TiltPos.ReadPOSITION();
    
  //  Tilt.SysActualVel = Tilt.System_position - Tilt.System_position_prev;

}

void AxisControlLoop(MyAxis& Axis)
{
    if (Axis.Number == 1) {
        UpdatePanFeedback();
    } else if (Axis.Number == 2) {
        UpdateTiltFeedback();
    }


    // ***********************************************************
    // Sinusoid test

    if ((Axis.sinusoidrun) && (Axis.jog > 0)) {
        //pc.printf("\n\r%d,%d,%f,%f",run_count,(Motor_Inst_Pos-262144),outprint,(b*1000));
        //Time (ms),Motor,System,SysDemandPos,frequency (Hz),Demand")
        pc.printf("\n\r%f,%f,%f,%f,%f",Axis.run_count,(Axis.Motor_position - Axis.start_MotPos),((Axis.System_position - Axis.start_SysPos)* Axis.ratio),(Hz*100),((Axis.SysDemandPos - Axis.start_SysPos) * Axis.ratio));

        if(Axis.run_count < (run_time * 1000)) {
            // demandpos = 262144 + (amplitude * sin((a*run_count*run_count)+(b*run_count)));
            Hz = (start_Hz + (a * Axis.run_count));  // This is the demand frequency
            //SysDemandPos = SysDemandPos + (amplitude * sin(2 * 3.14159 * Hz * run_count / 1000));
            Axis.SysDemandPos = Axis.start_SysPos + (amplitude * sin(2 * 3.14159 * Hz * Axis.run_count / 1000));
            // PanVelocityPID.setSetPoint(demandpos);
            Axis.run_count++;        // Inc run_count
        } else {
            Axis.sinusoidrun = 0;        // Stop the test
            Axis.run_count = 0;
        }
    }

    // ***********************************************************
    // Calculate the effective ratio on the fly

    if ((Axis.calcmyratio) && (fabs(Axis.SysActualVel) > 0)) {
       
        Axis.dynamicratio = fabs((Axis.Motor_Vel) / (Axis.SysActualVel));


    }

    // ***********************************************************
    // Go ahead and move at a set speed

    if (Axis.moveme) {
        Axis.SysDemandPos = Axis.SysDemandPos + (Axis.jog * Axis.sysjogdir);
    }

    // ***********************************************************
    // Create set ramps to follow
    // sysjog is System speed demand in counts/ms at the System encoder (output)
    if (Axis.setramp) {
        if (Axis.run_count_jog < jogaccel_time) {
            if (primed) {
                scoping = true;
                primed = false;
            }
            Axis.sysjogdir = 1;
            Axis.sysdojog = Axis.sysdojog + Axis.ramp;
            Axis.run_count_jog++;
            Axis.SysDemandPos = Axis.SysDemandPos + Axis.sysdojog;
            Axis.SysDemandPos = Axis.SysDemandPos * Axis.sysjogdir;
        } else if ((Axis.run_count_jog >= jogaccel_time) && (Axis.run_count_jog < (jogaccel_time + dwell))) {
            Axis.sysdojog = Axis.jog;
            Axis.run_count_jog++;
            Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
        } else if ((Axis.run_count_jog >= (jogaccel_time + dwell)) && (Axis.run_count_jog < ((2 * jogaccel_time) + dwell))) {
            Axis.sysdojog = Axis.sysdojog - Axis.ramp;
            Axis.run_count_jog++;
            Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
        } else if ((Axis.run_count_jog >= ((2 * jogaccel_time) + dwell)) && (Axis.run_count_jog < ((3 * jogaccel_time) + dwell))) {
            Axis.sysdojog = Axis.sysdojog + Axis.ramp;
            Axis.run_count_jog++;
            Axis.sysjogdir = -1;
            Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
        } else if ((Axis.run_count_jog >= ((3* jogaccel_time) + dwell)) && (Axis.run_count_jog < ((3* jogaccel_time) + dwell + dwell))) {
            Axis.sysdojog = 0;
            Axis.run_count_jog++;
        } else if (Axis.run_count_jog >= ((3* jogaccel_time) + dwell + dwell)) {
            Axis.setramp = false;
            Axis.run_count_jog = 0;
            Axis.sysjogdir = 1;
            scoping = false;
        }
    }

    if (Axis.setramp_x) {
        if (Axis.run_count_jog < jogaccel_time) {
            if (primed) {
                scoping = true;
                primed = false;
            }
            Axis.sysdojog = Axis.sysdojog + Axis.ramp;
            Axis.run_count_jog++;
            Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
        } else if ((Axis.run_count_jog >= jogaccel_time) && (Axis.run_count_jog < (jogaccel_time + dwell))) {
            Axis.sysdojog = Axis.jog;
            Axis.run_count_jog++;
            Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
        } else if ((Axis.run_count_jog >= (jogaccel_time + dwell)) && (Axis.run_count_jog < ((2 * jogaccel_time) + dwell))) {
            Axis.sysdojog = Axis.sysdojog - Axis.ramp;
            Axis.run_count_jog++;
            Axis.SysDemandPos = Axis.SysDemandPos + (Axis.sysdojog * Axis.sysjogdir);
        } else if ((Axis.run_count_jog >= ((2 * jogaccel_time) + dwell)) && (Axis.run_count_jog < ((2 * jogaccel_time) + dwell + dwell))) {
            Axis.sysdojog = 0;
            Axis.run_count_jog++;
        } else if (Axis.run_count_jog >= ((2* jogaccel_time) + dwell + dwell)) {
            Axis.setramp_x = false;
            Axis.run_count_jog = 0;
            scoping = false;
        }
    }


    // **********************************************************
    // Generate perfect velocity demand for the System:
    Axis.SysProfilerVel = Axis.SysDemandPos - Axis.SysDemand_Prev;
    Axis.SysDemand_Prev = Axis.SysDemandPos;

    // **********************************************************
    // Apply proportional gain to the System Velocity demand if sufficient
    Axis.Sys_follow_error = Axis.System_position - Axis.SysDemandPos;
    Axis.SysDerivedVel = Axis.SysProfilerVel - (Axis.Sys_follow_error * Axis.Kp_Sys);

    // ^ LOAD SIDE
    // *************************************************
    // MOTOR CONTROL LOOP
    // *************************************************
    // V MOTOR SIDE

    // Apply the system demand to the motor via the gearbox ratio:
    Axis.dojog = Axis.SysDerivedVel * Axis.ratio;

    //Derive a motor postion demand
    Axis.MotorDemandPos = Axis.MotorDemandPos + Axis.dojog;

    if (Axis.Motor_position <= -Motor_wrap) {
        // Motor is moving to wrap in a negative direction so bulk values to the mid point of the encoder
        Axis.Motor_position = Axis.Motor_position + (Motor_wrap);
        Axis.MotorDemandPos = Axis.MotorDemandPos + (Motor_wrap);
        //pc.printf("Wrap neg");
    } else if (Axis.Motor_position >= Motor_wrap) {
        // Motor is moving to wrap in a positive direction so bulk values back towards the mid point
        Axis.Motor_position = Axis.Motor_position - (Motor_wrap);
        Axis.MotorDemandPos = Axis.MotorDemandPos - (Motor_wrap);
        //pc.printf("Wrap pos");
    }

    // *********************************************************
    // Apply proportional position loop gain
    Axis.follow_error = Axis.MotorDemandPos - Axis.Motor_position;
    Axis.Motor_final = (Axis.Kp * Axis.follow_error);

    // *********************************************************
    // Apply feed forward profiling:
    // Gain Schedule according to system demand - this prevent stationary oscillation
    if (fabs(Axis.SysProfilerVel) > 0) {
        c = Axis.c_moving;
    } else {
        c = Axis.c_stationary;
    }
    m = (Axis.Jog_2 - c)/Axis.Jog_2;
    // Calculate the feed forward value
    if ((fabs(Axis.dojog) > (Axis.Jog_1 + 0)) && (fabs(Axis.dojog) <= Axis.Jog_2)) {
        Axis.Feed_Forward = ((m*fabs(Axis.dojog)) + c);
        if (Axis.dojog <= 0) {
            Axis.Feed_Forward = -Axis.Feed_Forward;
        }
    } else if (fabs(Axis.dojog) > Axis.Jog_2) {
        Axis.Feed_Forward = (Axis.Kff * Axis.dojog);
    } else {
        Axis.Feed_Forward = 0;
    }
    // Add the feedforward term to the demand already calculated:
    Axis.Motor_final = Axis.Motor_final + Axis.Feed_Forward;

    // *****************************************************
    // NOW PREPARE AND SEND DEMAND TO THE MOTOR
    // Apply scale factor from user units to motor duty cycle.
    Duty_Cycle = Axis.Motor_final / Axis.Motor_scale;

    if(Duty_Cycle < 0) {
        Motor_Direction = 1;
        Motor_PWM = 1 - ((Duty_Cycle - DeadBand) * -1.0);
    } else {
        Motor_Direction = 0;
        Motor_PWM = 1 - Duty_Cycle - DeadBand;
    }

    // Send demand to the (correct) motor:
    if (Axis.Number == 1) {
        Pan_Motor_Direction = Motor_Direction;
        Pan_Motor_PWM = Motor_PWM;
    } else if (Axis.Number == 2) {
        Tilt_Motor_Direction = Motor_Direction;
        Tilt_Motor_PWM = Motor_PWM;
    }

// ***************************************************
// CHOOSE VALUES AND SCOPE THEM
    if ((scoping) && (Axis.Number == 2)) {
        if (scope_time < 1) {
            // pc.printf("\n\r \n\rTime,Motor_position,follow_error,Sys_follow_error,dojog,SysProfilerVel,sysdojog,Feed_Forward");
            pc.printf("\n\r \n\rscope_time,Motor Error,Motor Virtual,Motor Position,System speed,Hall");
        }
        pc.printf("\n\r %d,%f,%f,%f,%f,%f",scope_time,Pan.follow_error, (Pan.follow_error / Pan.ratio),Pan.Motor_position, Pan.sysdojog, Pan_count);
        scope_time = scope_time + 1;
    }



}