File content as of revision 4:4dafa4113982:

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

// Define limits for zero crossing
// These values should allow operation upto 3750 RPM
#define bits 18                 // The number of bits we want to use
#define OneTurn (1<<bits)       // 262144 counts per rev
#define Lower (1<<(bits-5))     // 8192 counts = 11.25 degrees
#define Upper OneTurn - Lower   // 262144 - 8192 = 253952

extern Serial pc;
extern iC_MU tilt_ic_mu;
extern iC_MU TiltPos;
extern PwmOut Tilt_Motor_PWM;
extern DigitalOut Tilt_Motor_Direction;
extern Classic_PID TiltVelocityPID;
extern float Demand_Count_Rate;
extern float Tilt_motor_max_count_rate;
extern float Actual_Motor_Speed;

extern float T_Position; // True Tilt Position (Degrees)
extern float T_Encoder_sf;


int Last_M_Position = 0;
int M_Position = 0;
int Velocity = 0;
float Duty_Cycle = 0.0;

// System Configuration:
float MaxSpeed = 60.00; // 5 Deg/s, 1250 RPM
float Tilt_motor_max_count_rate = 6000; //encoder counts / ms
float T_Position = 0; // True Tilt Position (Degrees)
float T_Encoder_sf = 1456.355;  // counts per degree
float T_Gear_ratio = 125;  // 125:1
float M_Encoder = 262144; // counts/rev
float Motor_sf = ((1 * T_Gear_ratio) * (M_Encoder / 360))/1000 ;  // This is the scale factor from deg/s at payload to motor counts/ms
bool M_Encoder_front = true; // or 0 depending on the encoder configuration on the motor (facing to motor, or facing away from motor)

double drive = 0;  // this is a const speed demand straight into the loop (divided by 100)

// Main servo loop
int DoMove = 0;
const float LOOPs = 0.001;
// S_Ramp Fade values
float D = 10; // Fade distance
float T = 15; // Fade time
float dir = 1; //direction flag
float ta; // The actual value used after sanity checks
float ts; // The actual value used after sanity checks(S ramping)
float tsfade = 0.5; // segment time for S ramped fade
float tscut = 0.3; // segment time for S ramped cut
float j; // jerk value for fade
float aj; // accel value when S ramping
float Vp;  // Top speed for the move Deg/s @ load (125:1 Ratio to motor)
float Vs;  // Speed step increment
float Da;  // Accel distance
float Ds; // Distance convered at steady speed
float s;  // Profiler internal demand speed (always positive)
float sout;  // Demand as applied by the Vff term
float s_profile; // output demand speed to postion loop + or -)
double P;  // Profiler Demand postion
float fadetime; // this will retain the current fade time
float Error;    // Current position vs the profiler position
float Vff = 1;  // Velocity feedforward term - a value of 1 sends 100% profiler speed demand to motor
float T_Kp = 6;  //  This is is multiplied by the position error and added to the motor demand
float Prop;   // The demand created by the Kp and error calculation
float demand = 0;  // The value sento to the motor to make it move
float P_vel;
float Va; // mid speed point
float as; // acceleration value during linear accel stage
float Vj; // Speed at bottom intersection
float Vjp; // Speed at top intersection
float c; // constant for up ramp y=mx+c
float b; // constant for down ramp y = mx+b
float real_time;
float fade_tilt;
float joy_tilt;
float fade_time=10;
extern bool joystick;
extern float Time;

float T_Joy = 0.0;


void TiltVelocityLoop(void const *args)
{
    T_Position = 360 - (TiltPos.ReadPOSITION()/T_Encoder_sf); // the 3D printed unit counts the opposite way to the aluminium unit.

    if (joystick) {
        P = T_Position;
    }

    M_Position = tilt_ic_mu.ReadPOSITION() >> (19 - bits);// Read the current position from the iC-MU and bitshift to reduce noise
    Velocity = M_Position - Last_M_Position;             // Calculate change in position (i.e. Velocity)
    Actual_Motor_Speed = Velocity;


    // Check to see if we have gone past the index point
    if(M_Position < Lower & Last_M_Position > Upper) {       // We have gone over the index point in 1 direction
        Velocity += OneTurn;
    } else if(M_Position > Upper & Last_M_Position < Lower) {// We have gone over the index point in the other direction
        Velocity -= OneTurn;
    }
    Last_M_Position = M_Position;                            // Update new position from next time

    TiltVelocityPID.setProcessValue(Velocity);

    if(!joystick) {
        if (DoMove == 1) {
            if ((fadetime < ts) & (s < Vp)) {
                //led2 = 0;
                s = (j/2)*fadetime*fadetime;  //bottom parabola
                fadetime = fadetime + LOOPs; // This provides the base time for the fade sequence
            } else if ((fadetime >= ts) & (fadetime <(2*ts))) {
                s = (as*fadetime)+c; //steady accel stage
                fadetime = fadetime + LOOPs;
            } else if ((fadetime >= (2*ts)) & (fadetime <(3*ts))) {
                s = (-(j/2)*(fadetime-(3*ts))*(fadetime-(3*ts))) + Vp; // Top parabola
                fadetime = fadetime + LOOPs;
            } else if ((fadetime >= (3*ts)) & (fadetime <(T-(3*ts)))) {
                s = Vp;  // Steady Speed Stage
                fadetime = fadetime + LOOPs;
            } else if ((fadetime >= (T-(3*ts))) & (fadetime <(T-(2*ts)))) {
                s = (-(j/2)*(fadetime-(T-(3*ts)))*(fadetime-(T-(3*ts)))) + Vp; // Top parabola down
                fadetime = fadetime + LOOPs;
            } else if ((fadetime >= (T-ts-ts)) & (fadetime < (T-ts))) {
                s = -as*(fadetime - T) + c; //steady decel stage
                fadetime = fadetime + LOOPs;
            } else if ((fadetime >= (T-ts)) & (s < Vp) & (fadetime <= T)) {
                //led2 = 1;
                s = (j/2)*(T-fadetime)*(T-fadetime);  //bottom parabola to end
                fadetime = fadetime + LOOPs;
            } else if (fadetime >= T) {
                s=0;
                //led2 = 0;
                DoMove = 0;
                TiltVelocityPID.setSetPoint(0);
            } else {
                fadetime = fadetime + LOOPs; // for TBC reason this is needed!
            }
            // compute the new position demand:
            s_profile = s * dir;
            P = P + ((s_profile * LOOPs));
            real_time = ((T - fadetime) * 1000);
        } else {  // If not fading then respect joystick profiler instead

            P = P + (T_Joy * LOOPs);  // Sum total of Joystick profiler right now.
            P = P + (drive / 100000);  // Apply const speed drive

        }  // End of profilers

        sout = s_profile * Vff;  //Apply velocity feedforward term
        Error = (P - T_Position);    // Position Error
        Prop = T_Kp * Error;      // Calculate proportional gain element
        demand = sout + Prop;   // Sum the result of Vff and Kp to the demand
        P_vel = demand * Motor_sf; // Convert from load Deg/s into motor encoder counts/ms
        TiltVelocityPID.setSetPoint(-P_vel);
        //.printf("\n\r %f,   %f,   %f,   %f, %f",Time, s_profile, P_vel, T_Position, Error);
    }  // End of if !Joystick

    Duty_Cycle = TiltVelocityPID.compute_ff()/Tilt_motor_max_count_rate;

    if(Duty_Cycle < 0) {
        if (M_Encoder_front) {
            Tilt_Motor_Direction = 1;
        } else {
            Tilt_Motor_Direction = 0;
        }
        Tilt_Motor_PWM = 1 - (Duty_Cycle * -1.0);
    } else {
        if (M_Encoder_front) {
            Tilt_Motor_Direction = 0;
        } else {
            Tilt_Motor_Direction = 1;
        }
        Tilt_Motor_PWM = 1 - Duty_Cycle;
    }
}

void Profile() // For S ramped movement using Servo for S ramping. Calling here as all it's variables are needed in this arena
{
    if ((fade_tilt >=0) & (fade_tilt <= 359)) {
        D = fade_tilt - T_Position; // Calculate distance to move
    } else {
        D = 0;
        abort();  // leave this function
        // add an error event handler here
    }

    if (D <= 0) {
        dir = -1;
        D = abs(D);
    } else {
        dir = 1;
    }

    if (fade_time <= (6*tsfade + 0.2)) {
        ts = tscut;
        if (fade_time <= (6*tscut+0.2)) {
            T = 6*tscut + 0.2;  //min fade fime
        }

    } else {
        ts = tsfade;
        T = fade_time;
    }
   
    Vp = D / (T-(3*ts));  // Equation 1
    if (Vp > MaxSpeed) {         //Check for maximum speed condition
        Vp = MaxSpeed;           //Do the fade as fast as possible
        T = (D + (Vp * (3*ts)))/Vp;
    }

    // New version based on S-Ramping Doc - V2

    j = Vp / (2*ts*ts);
    as = j * ts;
    c = -(Vp / 4);
    s = 0;
    fadetime = 0;
    // Time = 0;
    P = T_Position;

}