File content as of revision 2:4173f8896073:

/*  Version 2.7, Opened 4/03/2015
This Program closes a servo loop on the FHR35 Pan axis. This is using an SSI encoder for
Position feedback and a Copley Amplifier driven in Velocity mode.*/
/*
0. Created around 04/03/2013
1. Basic Profiler working
2. Added SSI encoder support
3. Added Proportional Gain
4. Encoder zero function
5. Merged Ethernet comms
6. Motor Enable function
7. Execute fades in both directons from Comms  30/03/13
8. Added error traps for max fade speed and keep within 0-360 for fade demand. Reports IP Address on LCD on startup
9. Added S Ramping Code 8 July 2013
10. Added proper Vff and control of Vff and Kp from parent control, July 2013
11. Improved S Ramp according to Diogo's V2 document - previous maths was not working, April 2015
12. Added Tilt Axis as virtual axis for sync and other testing, 13/04/2015
*/
#include "mbed.h"
#include "TextLCD.h"
#include "EthernetInterface.h"

DigitalOut led1(LED1);  //motor enabled
DigitalOut led2(LED2);  //Fade or cut
DigitalOut led3(LED3);
DigitalOut led4(LED4);  //Connected

DigitalIn Run(p16);
TextLCD lcd(p25, p26, p27, p28, p29, p30, TextLCD::LCD16x2);

EthernetInterface eth; //, ethMask, ethNull, ethNull);
TCPSocketServer sock;
TCPSocketConnection client;

LocalFileSystem local("local");  // Create the local filesystem

Serial pc(USBTX, USBRX);
SPI spi(p5, p6, p7);  // 5= MOSI  6 =MOSI  7 =CLK

// p5 N/C
// p6 MOSI -> Pin 5 on encoder
// P7 CLK  -> Pin 3 on encoder

//Copley Drive Constants
AnalogOut Cop(p18);
DigitalOut Enable(p20);

float AOffset = 0.5;
float AMax = 0.4;
float MaxSpeed = 5.00; // Deg/s, 1250 RPM
float Aout = AOffset;

//Networking values
typedef struct _data {
    int pan;
    int tilt;
    int time;
    int speed;
    int error;
} data;
data real;

typedef struct _data2 {
    float pan;
    float tilt;
    float time;
} data2;
data2 fade;
data2 joy;

char InBound[100]; // incomming data
char OutBound[100]; // outbound data
char outpan[10];
char outtilt[10];
char outtime[10];
char outspeed[10];
char outerror[10];
char header[10]; //= "real ";
char space[] = " ";
int checksum, pan, tilt, duration, jpan, jtilt, command, newkp, newvff;

// Encoder reading:
int High, Low;
long Encoder;
float Angle;
float Offset = 0;

// Main servo loop
int DoMove = 0;
int Stopped = 0; //1 means that a stop command has been received and not cleared.
//const int LOOPus = 5000; // This will be the parent loop time (microsec)
const int LOOPms = 5;
//Ticker ServoLoop;
int counter = 10; //This defines the base count for a sub-loop to stagger non-servo critical events
// Profile Calculation:
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 tafade = 2; // accel time for a fade
float ts; // The actual value used after sanity checks(S ramping)
float tsfade = 0.5; // segment time for S ramped fade
float tscut = 0.2; // segment time for S ramped cut
float j; // jerk value for fade
float aj; // accel value when S ramping
float tacut = 1; // accel time for a cut
float Vp;  // Top speed for the move Deg/s @ load (256: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 -)
float P;  // Profiler Demand postion
float Steps; // number of steps through the accel ramps (trapezoidal profile)
float fadetime; // this will retain the current fade time
float Error;    // Current position vs the profiler position
float Vff =0.9; // Velocity feedforward term - a value of 1 sends 100% profiler speed demand to motor
float Kp = 7;  //  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 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

void Connectme()
{
    eth.init("198.168.1.20","255.255.255.0","0.0.0.0");
    eth.connect();
    lcd.locate(0,1);
    lcd.printf("%s\n", eth.getIPAddress());
    if (sock.bind(80)>=0) {
        led4 = 1;
    }
    sock.listen(1);
    while (!client.is_connected()) {
        sock.accept(client);
    }
    led4 = 1;
}

void Profile() // For S ramped movement using Servo for S ramping
{
    if ((fade.pan >=0) & (fade.pan <= 359)) {
        D = fade.pan - Angle; // 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;
        T = fade.time;
    } else {
        ts = tsfade;
        T = fade.time;
    }
    if (fade.time <= (6*tscut+0.2)) {
        T = 6*tscut + 0.2;  //min fade fime
    }

    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;
    //P = 0; //Angle;
}

void BuildOut(data outdata)
{
    htons(outdata.pan);
    htons(outdata.tilt);
    htons(outdata.time);
    htons(outdata.speed);
    htons(outdata.error);
    sprintf(outpan, "%d", outdata.pan);
    sprintf(outtilt, "%d", outdata.tilt);
    sprintf(outtime, "%d", outdata.time);
    sprintf(outspeed, "%d", outdata.speed);
    sprintf(outerror, "%d", outdata.error);
    strncpy(OutBound, header, sizeof(OutBound)-1);
    strcat(OutBound, space);
    strcat(OutBound, outpan);
    strcat(OutBound, space);
    strcat(OutBound, outtilt);
    strcat(OutBound, space);
    strcat(OutBound, outtime);
    strcat(OutBound, space);
    strcat(OutBound, outspeed);
    strcat(OutBound, space);
    strcat(OutBound, outerror);
    strcat(OutBound, space);
}

void BreakIn()
{
    sscanf(InBound, "%d %d %d %d %d %d %d %d %d", &checksum, &pan, &tilt, &duration, &jpan, &jtilt, &command, &newkp, &newvff);
    ntohl(checksum);
    ntohl(pan);
    ntohl(tilt);
    ntohl(duration);
    ntohl(jpan);
    ntohl(jtilt);
    ntohl(command);
    ntohl(newkp);
    ntohl(newvff);
    if (checksum == (pan + tilt)) {
        joy.pan = jpan;
        joy.tilt = jtilt;
    } else {
        command = -1; //Flag the failure
    }
    switch (command) {
        case 1: //Fade
            if (DoMove != 1) {
                fade.pan = pan;
                fade.pan = fade.pan/1000;
                fade.tilt = tilt;
                fade.tilt = fade.tilt/1000;
                fade.time = duration;
                fade.time = fade.time/1000;
                sprintf(header, "%s", "Fade");
                Profile();
                DoMove = 1;
            } else {
                sprintf(header, "%s", "No_Moving");
            }
            break;
        case 3:  // Enable
            if (Enable != 1) {
                Stopped = 0;
                P = Angle;
                Enable = 1;
                led1 = 1;
                sprintf(header, "%s", "Enable");
            } else {
                Enable = 0;
                led1 = 0;
                sprintf(header, "%s", "Disabled");
            }
            break;
        case 10: //Polling
            real.pan = (Angle * 1000);
            real.speed = (sout * 1000);
            real.error = (Error * 1000);
            //real.pan = (P * 1000);
            if ((Enable != 1) && (Stopped !=1)) {
                sprintf(header, "%s", "Disabled");
            }  else if (DoMove != 0) {
                sprintf(header, "%s", "Fading");
            }  else if (Stopped != 0) {
                sprintf(header, "%s", "Stopped");
            } else if (s_profile !=0) {
                sprintf(header, "%s", "Moving");
            } else {
                sprintf(header, "%s", "Idle");
            }
            break;
        case 20:  // Change Values  
            Kp = newkp;        
            Kp = Kp/1000;
            Vff = newvff;
            Vff = Vff/1000;
            sprintf(header, "%s", "Accepted");
            break;
        case 90:  //Stop
            sprintf(header, "%s", "Stop");
            Stopped = 1;  //This flags that we have had a stop and must be pro-actively cleared.
            DoMove = 0;  //This will stop a fade in progress machinery
            Enable = 0;  //Disable the motor drives
            break;
        default:
            sprintf(header, "%s", "Error");
    }
}

void Comms_thread(void const *args)
{
    while(true) {
        if (client.receive(InBound, sizeof(InBound)-1) >=0) {
            BreakIn();
            BuildOut(real);
            client.send(OutBound, sizeof(OutBound));
        }
        Thread::wait(8);
    }
}

void LCD_thread(void const *args)
{
    while (true) {
        lcd.cls();
        pc.printf("A=%7.4f, P= %f, D= %f, T= %f,\r",Angle,P,D,T);
        //lcd.printf("%3.2f,%3.2f,%3.2f", Angle,P,demand);
        lcd.locate(0,0);
        lcd.printf("A=%f", Angle);
        //lcd.printf("J=%f", joy.pan);
        lcd.locate(0,1);
        lcd.printf("s=%f", s_profile);
        Thread::wait(32);
    }
}

void DriveMotor(float speed)
{
    Aout = (speed/MaxSpeed) + AOffset;
    Cop = Aout;
}

void ReadEncoder()
{
    High = spi.write(0x0000);           //Write dummy value to read 16 MSB
    Low = spi.write(0x0000);            //Write dummy value to read 16 LSB
    Encoder=(High<<16)+Low;
    Encoder=(Encoder & 0x1FFFFF80);
    Encoder=(Encoder>>7);
    Angle = (Encoder * 0.0000858306885);
    Angle = Angle + Offset;
    if (Angle < 0) {
        Angle = Angle + 360;
    }
}
void ZeroEncoder()
{
    High = spi.write(0x0000);           //Write dummy value to read 16 MSB
    Low = spi.write(0x0000);            //Write dummy value to read 16 LSB
    Encoder=(High<<16)+Low;
    Encoder=(Encoder & 0x1FFFFF80);
    Encoder=(Encoder>>7);
    Angle = Encoder * 0.0000858306885;
    Offset = (Angle * -1)+180;
    pc.printf("Zero conplete %7.4f, ,%7.4f",Angle, Offset);
    Angle = Angle + Offset;
    if (Angle < 0) {
        Angle = Angle + 360;
    }
    pc.printf(" New A=%7.4f   ",Angle);
    P = Angle;
    wait(0.1);
}

void Servo(void const *args)  //This is threaded with real-time priority.
// This is S ramped servo control based on the S ramp Profile calculations
{
    while(true) {
        ReadEncoder();
        if ((DoMove == 1) & (Enable != 0) & (Stopped !=1)) {
            if ((fadetime < ts) & (s < Vp)) {
                led2 = 0;
                s = (j/2)*fadetime*fadetime;  //bottom parabola
                fadetime = fadetime + 0.005; // 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 + 0.005;
            } else if ((fadetime >= (2*ts)) & (fadetime <(3*ts))) {
                s = (-(j/2)*(fadetime-(3*ts))*(fadetime-(3*ts))) + Vp; // Top parabola
                fadetime = fadetime + 0.005;
            } else if ((fadetime >= (3*ts)) & (fadetime <(T-(3*ts)))) {
                s = Vp;  // Steady Speed Stage
                fadetime = fadetime + 0.005;
            } 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 + 0.005;
            } else if ((fadetime >= (T-ts-ts)) & (fadetime < (T-ts))) {
                s = -as*(fadetime - T) + c; //steady decel stage
                fadetime = fadetime + 0.005;
            } else if ((fadetime >= (T-ts)) & (s < Vp) & (fadetime <= T)) {
                led2 = 1;
                s = (j/2)*(T-fadetime)*(T-fadetime);  //bottom parabola to end
                fadetime = fadetime + 0.005;
            } else if (fadetime >= T) {
                s=0;
                led2 = 0;
                DoMove = 0;
            } else {
                fadetime = fadetime + 0.005; // for TBC reason this is needed!
            }
            // compute the new position demand:
            s_profile = s * dir;
            P = P + (s_profile * 0.005);
            real.time = ((T - fadetime) * 1000);
        } else {
            DoMove = 0;
            real.time = 0;
        }   // end of DoMove (Fade or Cut

        if ((Enable !=0) & (Stopped !=1)) {  //This executes the core postion loop.
            if (DoMove == 0) {               //Adding Joystick Demand as appropriate
                s_profile = MaxSpeed * (joy.pan/100);
                P = P + (s_profile * 0.005);
            } else {
                joy.pan = 0;
                joy.tilt = 0;
            }
            sout = s_profile * Vff;  //Apply velocity feedforward term
            Error = (P - Angle);    // Position Error
            Prop = Kp * Error;      // Calculate proportional gain element
            demand = sout + Prop;   // Sum the result of Vff and Kp to the demand
            DriveMotor(demand);     // Send the demand to the motor
        } else {
            DriveMotor(0);
        }  //End of core position loop
        Thread::wait(5);
    }
}


int main()
{

    Enable = 0;    // Try to prevent movement on startup
    DriveMotor(0); // Drive demand to Zero
    lcd.cls();
    lcd.printf("2.7: Connecting");


    pc.baud(115200);            //Pimp the baud rate

    spi.format(16,2);           // Setup the spi for 14 bit data, high steady state clock, read on rising edge
    spi.frequency(400000);      //200kHz
    ReadEncoder();
    pc.printf("1st Read says  %7.4f   ",Angle);
    ZeroEncoder();

    ReadEncoder();
    pc.printf("Read says  %7.4f   ",Angle);

    Connectme();                //Connect to the outside world


    Thread thread_COMMS(Comms_thread);  //Start reading from the network.
    Thread thread_LCD(LCD_thread);  //Start updating the LCD

    Thread thread_Servo(Servo);
    thread_Servo.set_priority(osPriorityRealtime);

    //Profile();
    wait(2);

    while(1) {
  
    }
}