Dependencies: EthernetInterface TextLCD mbed-rtos mbed
Fork of
1_System_Controller
by 
Andrew Codd
Diff: main.cpp
- Revision:
- 1:120eeae4d43a
- Parent:
- 0:ce4a539ca1d4
- Child:
- 2:4173f8896073
--- a/main.cpp Fri Oct 22 17:36:47 2010 +0000
+++ b/main.cpp Wed Feb 04 10:35:25 2015 +0000
@@ -1,40 +1,588 @@
-/* This program is designed to interface with the AS5045 IC encoder
- It calls a function to read the encoder value every second
- The program then prints the vaule to a PC screen via hyperterminal*/
+/* Version 3.0, Opened 4/03/2013
+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.*/
+/*
+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
+*/
+#include "mbed.h"
+#include "TextLCD.h"
+#include "EthernetInterface.h"
-#include "mbed.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); // mosi, miso, sclk
-DigitalOut cs(p25);
+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;
+ int 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 Vt; // 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
-long EncoderValue;
-int Encoder;
+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_Trap()
+{
+ 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 <= (2*tafade + 0.2)) {
+ ta = tacut;
+ T = fade.time;
+ } else {
+ ta = tafade;
+ T = fade.time;
+ }
+ if (fade.time <= (2*tacut+0.2)) {
+ T = 2*tacut + 0.2; //min fade fime is 2xcut acceleration for now
+ }
-Ticker ReadEncoder;
+ Vt = D / (T-ta);
+ if (Vt > MaxSpeed) { //Check for maximum speed condition
+ Vt = MaxSpeed; //Do the fade as fast as possible
+ T = (D + (Vt * ta))/Vt;
+ }
+ Da = (Vt/2)*ta;
+ Ds = Vt * (T - (2*ta));
+ Steps = (ta*1000) / LOOPms;
+ Vs = Vt / Steps;
+ s = 0;
+ fadetime = 0;
+ //P = 0; //Angle;
+}
+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
+ }
-void Read(){
+ 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 <= (2*tscut+0.2)) {
+ T = 6*tscut + 0.2; //min fade fime
+ }
+
+ Vt = D / (T-(3*ts)); // Equation 1
+ if (Vt > MaxSpeed) { //Check for maximum speed condition
+ Vt = MaxSpeed; //Do the fade as fast as possible
+ T = (D + (Vt * (3*ts)))/Vt;
+ }
+ // change more here
+ Va =(Vt / 2); // Equation 2
+
+ j = ( ((2*Va)-(1.5*ts))/(3*ts*ts)); // Eqation 3
- spi.format(9,2); // Setup the spi for 9 bit data, high steady state clock,
- spi.frequency(500000);
- cs = 0; // Select the device by seting chip select low
+ Vj = (j * ts * ts); // Equation 5
+
+ Vjp = Vt - Vj; // Equation 6
+
+ as = (Vjp-Vj)/ts; // Equation 4
+
+ c = Vj - (as*ts); // Equation 7
+
+ b = Vj - (-as*(T-ts)); // Equation 8
+
+ 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);
+}
-// Send a dummy byte to receive the contents encoder
- int upper = spi.write(0x00);
- int lower = spi.write(0x00);
- lower = lower >> 6;
- upper = (upper << 6)+lower;
- cs = 1;
-
- pc.printf("%d \r", upper);
+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;
+ 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);
}
-int main() {
- pc.printf("Hello World!\n");
- ReadEncoder.attach_us(&Read, 100000); // call flip every 100 milli-seconds
-
- while(1) {
+void Servo_Trap(void const *args) //This is threaded with real-time priority.
+{
+ while(true) {
+ ReadEncoder();
+ if ((DoMove == 1) & (Enable != 0) & (Stopped !=1)) {
+ if ((fadetime < ta) & (s < Vt)) {
+ led2 = 1;
+ s = s + Vs;
+ fadetime = fadetime + 0.005; // This provides the base time for the fade sequence
+ } else if ((fadetime >= ta) & (fadetime <(T-ta))) {
+ s = Vt;
+ fadetime = fadetime + 0.005;
+ } else if ((fadetime >= (T-ta)) & (fadetime < T)) {
+ s = s-Vs;
+ 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;
+ }
+ Error = (P - Angle);
+ Prop = Kp * Error;
+ demand = s_profile+Prop;
+ DriveMotor(demand);
+ } else {
+ DriveMotor(0);
+ } //End of core position loop
+ Thread::wait(5);
}
}
+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 < Vt)) {
+ led2 = 0;
+ s = j*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*(fadetime-(3*ts))*(fadetime-(3*ts))) + Vt; // Top parabola
+ fadetime = fadetime + 0.005;
+ } else if ((fadetime >= (3*ts)) & (fadetime <(T-(3*ts)))) {
+ s = Vt; // Steady Speed Stage
+ fadetime = fadetime + 0.005;
+ } else if ((fadetime >= (T-(3*ts))) & (fadetime <(T-(2*ts)))) {
+ s = (-j*(fadetime-(T-(3*ts)))*(fadetime-(T-(3*ts)))) + Vt; // Top parabola down
+ fadetime = fadetime + 0.005;
+ } else if ((fadetime >= (T-ts-ts)) & (fadetime < (T-ts))) {
+ s = (-as*fadetime)+b; //steady decel stage
+ fadetime = fadetime + 0.005;
+ } else if ((fadetime >= (T-ts)) & (s < Vt) & (fadetime <= T)) {
+ led2 = 1;
+ s = j*(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.5: 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) {
+ // if (Run == 1) {
+ // wait(0.01); //de-bounce
+ // if (Run == 1) {
+
+ // DoMove = 1;
+ // }
+ // }
+ }
+}
+/*
+FILE *fp = fopen("/local/log.txt", "a"); // Open "log.txt" on the local file system for append
+ fprintf(fp,"Angle %f\r\n",Angle); //
+ fclose(fp);
+ wait(2);
+ ReadEncoder();
+ fopen("/local/log.txt", "a"); // Open "log.txt" on the local file system for append
+ fprintf(fp,"Angle %f\r\n",Angle); //
+ fclose(fp);
+void ReadFile (void) {
+ FILE *set = fopen("/local/setup.txt", "r"); // Open "setup.txt" on the local file system for read
+ fscanf(set,"%s %d",A,&offset); // read offset
+ fscanf(set,"%s %f",B,&gain); // read gain
+ fclose(set);
+}
+void WriteFile(void) { // write to USB memory
+ FILE *fp = fopen("/local/setup.txt", "a"); // Open "setup.txt" on the local file system for append
+ fprintf(fp,"offset %d\r\n",Angle); //
+ fprintf(fp,"gain %f\r\n",s); //
+ fclose(fp);
+}
+*/
+