Andrew Codd / Mbed 2 deprecated 1_System_Controller

Ethernet enabled servo loop using the RTOS and custom servo and encoder hardware

Dependencies:   EthernetInterface TextLCD mbed-rtos mbed

Fork of AS5045_Encoder_SPI by Martin Smith

main.cpp@1:120eeae4d43a, 2015-02-04 (annotated)

Committer:
acodd
Date:
Wed Feb 04 10:35:25 2015 +0000
Revision:
1:120eeae4d43a
Parent:
0:ce4a539ca1d4 
First Release of servo controller with Ethernet, RTOS and S Ramping

Who changed what in which revision?

UserRevisionLine numberNew contents of line
acodd 1:120eeae4d43a 1 /* Version 3.0, Opened 4/03/2013
acodd 1:120eeae4d43a 2 This Program closes a servo loop on the FHR35 Pan axis. This is using an SSI encoder for
acodd 1:120eeae4d43a 3 Position feedback and a Copley Amplifier driven in Velocity mode.*/
acodd 1:120eeae4d43a 4 /*
acodd 1:120eeae4d43a 5 1. Basic Profiler working
acodd 1:120eeae4d43a 6 2. Added SSI encoder support
acodd 1:120eeae4d43a 7 3. Added Proportional Gain
acodd 1:120eeae4d43a 8 4. Encoder zero function
acodd 1:120eeae4d43a 9 5. Merged Ethernet comms
acodd 1:120eeae4d43a 10 6. Motor Enable function
acodd 1:120eeae4d43a 11 7. Execute fades in both directons from Comms 30/03/13
acodd 1:120eeae4d43a 12 8. Added error traps for max fade speed and keep within 0-360 for fade demand. Reports IP Address on LCD on startup
acodd 1:120eeae4d43a 13 9. Added S Ramping Code 8 July 2013
acodd 1:120eeae4d43a 14 10. Added proper Vff and control of Vff and Kp from parent control, July 2013
acodd 1:120eeae4d43a 15 */
acodd 1:120eeae4d43a 16 #include "mbed.h"
acodd 1:120eeae4d43a 17 #include "TextLCD.h"
acodd 1:120eeae4d43a 18 #include "EthernetInterface.h"
ms523 0:ce4a539ca1d4 19
acodd 1:120eeae4d43a 20 DigitalOut led1(LED1); //motor enabled
acodd 1:120eeae4d43a 21 DigitalOut led2(LED2); //Fade or cut
acodd 1:120eeae4d43a 22 DigitalOut led3(LED3);
acodd 1:120eeae4d43a 23 DigitalOut led4(LED4); //Connected
acodd 1:120eeae4d43a 24
acodd 1:120eeae4d43a 25 DigitalIn Run(p16);
acodd 1:120eeae4d43a 26 TextLCD lcd(p25, p26, p27, p28, p29, p30, TextLCD::LCD16x2);
acodd 1:120eeae4d43a 27
acodd 1:120eeae4d43a 28 EthernetInterface eth; //, ethMask, ethNull, ethNull);
acodd 1:120eeae4d43a 29 TCPSocketServer sock;
acodd 1:120eeae4d43a 30 TCPSocketConnection client;
acodd 1:120eeae4d43a 31
acodd 1:120eeae4d43a 32 LocalFileSystem local("local"); // Create the local filesystem
ms523 0:ce4a539ca1d4 33
ms523 0:ce4a539ca1d4 34 Serial pc(USBTX, USBRX);
acodd 1:120eeae4d43a 35 SPI spi(p5, p6, p7); // 5= MOSI 6 =MOSI 7 =CLK
acodd 1:120eeae4d43a 36
acodd 1:120eeae4d43a 37 // p5 N/C
acodd 1:120eeae4d43a 38 // p6 MOSI -> Pin 5 on encoder
acodd 1:120eeae4d43a 39 // P7 CLK -> Pin 3 on encoder
acodd 1:120eeae4d43a 40
acodd 1:120eeae4d43a 41 //Copley Drive Constants
acodd 1:120eeae4d43a 42 AnalogOut Cop(p18);
acodd 1:120eeae4d43a 43 DigitalOut Enable(p20);
acodd 1:120eeae4d43a 44
acodd 1:120eeae4d43a 45 float AOffset = 0.5;
acodd 1:120eeae4d43a 46 float AMax = 0.4;
acodd 1:120eeae4d43a 47 float MaxSpeed = 5.00; // Deg/s, 1250 RPM
acodd 1:120eeae4d43a 48 float Aout = AOffset;
acodd 1:120eeae4d43a 49
acodd 1:120eeae4d43a 50 //Networking values
acodd 1:120eeae4d43a 51 typedef struct _data {
acodd 1:120eeae4d43a 52 int pan;
acodd 1:120eeae4d43a 53 int tilt;
acodd 1:120eeae4d43a 54 int time;
acodd 1:120eeae4d43a 55 int speed;
acodd 1:120eeae4d43a 56 int error;
acodd 1:120eeae4d43a 57 } data;
acodd 1:120eeae4d43a 58 data real;
acodd 1:120eeae4d43a 59
acodd 1:120eeae4d43a 60 typedef struct _data2 {
acodd 1:120eeae4d43a 61 float pan;
acodd 1:120eeae4d43a 62 float tilt;
acodd 1:120eeae4d43a 63 int time;
acodd 1:120eeae4d43a 64 } data2;
acodd 1:120eeae4d43a 65 data2 fade;
acodd 1:120eeae4d43a 66 data2 joy;
acodd 1:120eeae4d43a 67
acodd 1:120eeae4d43a 68 char InBound[100]; // incomming data
acodd 1:120eeae4d43a 69 char OutBound[100]; // outbound data
acodd 1:120eeae4d43a 70 char outpan[10];
acodd 1:120eeae4d43a 71 char outtilt[10];
acodd 1:120eeae4d43a 72 char outtime[10];
acodd 1:120eeae4d43a 73 char outspeed[10];
acodd 1:120eeae4d43a 74 char outerror[10];
acodd 1:120eeae4d43a 75 char header[10]; //= "real ";
acodd 1:120eeae4d43a 76 char space[] = " ";
acodd 1:120eeae4d43a 77 int checksum, pan, tilt, duration, jpan, jtilt, command, newkp, newvff;
acodd 1:120eeae4d43a 78
acodd 1:120eeae4d43a 79 // Encoder reading:
acodd 1:120eeae4d43a 80 int High, Low;
acodd 1:120eeae4d43a 81 long Encoder;
acodd 1:120eeae4d43a 82 float Angle;
acodd 1:120eeae4d43a 83 float Offset = 0;
acodd 1:120eeae4d43a 84
acodd 1:120eeae4d43a 85 // Main servo loop
acodd 1:120eeae4d43a 86 int DoMove = 0;
acodd 1:120eeae4d43a 87 int Stopped = 0; //1 means that a stop command has been received and not cleared.
acodd 1:120eeae4d43a 88 //const int LOOPus = 5000; // This will be the parent loop time (microsec)
acodd 1:120eeae4d43a 89 const int LOOPms = 5;
acodd 1:120eeae4d43a 90 //Ticker ServoLoop;
acodd 1:120eeae4d43a 91 int counter = 10; //This defines the base count for a sub-loop to stagger non-servo critical events
acodd 1:120eeae4d43a 92 // Profile Calculation:
acodd 1:120eeae4d43a 93 float D = 10; // Fade distance
acodd 1:120eeae4d43a 94 float T = 15; // Fade time
acodd 1:120eeae4d43a 95 float dir = 1; //direction flag
acodd 1:120eeae4d43a 96 float ta; // The actual value used after sanity checks
acodd 1:120eeae4d43a 97 float tafade = 2; // accel time for a fade
acodd 1:120eeae4d43a 98 float ts; // The actual value used after sanity checks(S ramping)
acodd 1:120eeae4d43a 99 float tsfade = 0.5; // segment time for S ramped fade
acodd 1:120eeae4d43a 100 float tscut = 0.2; // segment time for S ramped cut
acodd 1:120eeae4d43a 101 float j; // jerk value for fade
acodd 1:120eeae4d43a 102 float aj; // accel value when S ramping
acodd 1:120eeae4d43a 103 float tacut = 1; // accel time for a cut
acodd 1:120eeae4d43a 104 float Vt; // Top speed for the move Deg/s @ load (256:1 Ratio to motor)
acodd 1:120eeae4d43a 105 float Vs; // Speed step increment
acodd 1:120eeae4d43a 106 float Da; // Accel distance
acodd 1:120eeae4d43a 107 float Ds; // Distance convered at steady speed
acodd 1:120eeae4d43a 108 float s; // Profiler internal demand speed (always positive)
acodd 1:120eeae4d43a 109 float sout; // Demand as applied by the Vff term
acodd 1:120eeae4d43a 110 float s_profile; // output demand speed to postion loop + or -)
acodd 1:120eeae4d43a 111 float P; // Profiler Demand postion
acodd 1:120eeae4d43a 112 float Steps; // number of steps through the accel ramps (trapezoidal profile)
acodd 1:120eeae4d43a 113 float fadetime; // this will retain the current fade time
acodd 1:120eeae4d43a 114 float Error; // Current position vs the profiler position
acodd 1:120eeae4d43a 115 float Vff =0.9; // Velocity feedforward term - a value of 1 sends 100% profiler speed demand to motor
acodd 1:120eeae4d43a 116 float Kp = 7; // This is is multiplied by the position error and added to the motor demand
acodd 1:120eeae4d43a 117 float Prop; // The demand created by the Kp and error calculation
acodd 1:120eeae4d43a 118 float demand = 0; // The value sento to the motor to make it move
acodd 1:120eeae4d43a 119 float Va; // mid speed point
acodd 1:120eeae4d43a 120 float as; // acceleration value during linear accel stage
acodd 1:120eeae4d43a 121 float Vj; // Speed at bottom intersection
acodd 1:120eeae4d43a 122 float Vjp; // Speed at top intersection
acodd 1:120eeae4d43a 123 float c; // constant for up ramp y=mx+c
acodd 1:120eeae4d43a 124 float b; // constant for down ramp y = mx+b
ms523 0:ce4a539ca1d4 125
acodd 1:120eeae4d43a 126 void Connectme()
acodd 1:120eeae4d43a 127 {
acodd 1:120eeae4d43a 128 eth.init("198.168.1.20","255.255.255.0","0.0.0.0");
acodd 1:120eeae4d43a 129 eth.connect();
acodd 1:120eeae4d43a 130 lcd.locate(0,1);
acodd 1:120eeae4d43a 131 lcd.printf("%s\n", eth.getIPAddress());
acodd 1:120eeae4d43a 132 if (sock.bind(80)>=0) {
acodd 1:120eeae4d43a 133 led4 = 1;
acodd 1:120eeae4d43a 134 }
acodd 1:120eeae4d43a 135 sock.listen(1);
acodd 1:120eeae4d43a 136 while (!client.is_connected()) {
acodd 1:120eeae4d43a 137 sock.accept(client);
acodd 1:120eeae4d43a 138 }
acodd 1:120eeae4d43a 139 led4 = 1;
acodd 1:120eeae4d43a 140 }
acodd 1:120eeae4d43a 141 void Profile_Trap()
acodd 1:120eeae4d43a 142 {
acodd 1:120eeae4d43a 143 if ((fade.pan >=0) & (fade.pan <= 359)) {
acodd 1:120eeae4d43a 144 D = fade.pan - Angle; // Calculate distance to move
acodd 1:120eeae4d43a 145 } else {
acodd 1:120eeae4d43a 146 D = 0;
acodd 1:120eeae4d43a 147 abort(); // leave this function
acodd 1:120eeae4d43a 148 // add an error event handler here
acodd 1:120eeae4d43a 149 }
acodd 1:120eeae4d43a 150
acodd 1:120eeae4d43a 151 if (D <= 0) {
acodd 1:120eeae4d43a 152 dir = -1;
acodd 1:120eeae4d43a 153 D = abs(D);
acodd 1:120eeae4d43a 154 } else {
acodd 1:120eeae4d43a 155 dir = 1;
acodd 1:120eeae4d43a 156 }
acodd 1:120eeae4d43a 157
acodd 1:120eeae4d43a 158 if (fade.time <= (2*tafade + 0.2)) {
acodd 1:120eeae4d43a 159 ta = tacut;
acodd 1:120eeae4d43a 160 T = fade.time;
acodd 1:120eeae4d43a 161 } else {
acodd 1:120eeae4d43a 162 ta = tafade;
acodd 1:120eeae4d43a 163 T = fade.time;
acodd 1:120eeae4d43a 164 }
acodd 1:120eeae4d43a 165 if (fade.time <= (2*tacut+0.2)) {
acodd 1:120eeae4d43a 166 T = 2*tacut + 0.2; //min fade fime is 2xcut acceleration for now
acodd 1:120eeae4d43a 167 }
ms523 0:ce4a539ca1d4 168
acodd 1:120eeae4d43a 169 Vt = D / (T-ta);
acodd 1:120eeae4d43a 170 if (Vt > MaxSpeed) { //Check for maximum speed condition
acodd 1:120eeae4d43a 171 Vt = MaxSpeed; //Do the fade as fast as possible
acodd 1:120eeae4d43a 172 T = (D + (Vt * ta))/Vt;
acodd 1:120eeae4d43a 173 }
acodd 1:120eeae4d43a 174 Da = (Vt/2)*ta;
acodd 1:120eeae4d43a 175 Ds = Vt * (T - (2*ta));
acodd 1:120eeae4d43a 176 Steps = (ta*1000) / LOOPms;
acodd 1:120eeae4d43a 177 Vs = Vt / Steps;
acodd 1:120eeae4d43a 178 s = 0;
acodd 1:120eeae4d43a 179 fadetime = 0;
acodd 1:120eeae4d43a 180 //P = 0; //Angle;
acodd 1:120eeae4d43a 181 }
acodd 1:120eeae4d43a 182 void Profile() // For S ramped movement using Servo for S ramping
acodd 1:120eeae4d43a 183 {
acodd 1:120eeae4d43a 184 if ((fade.pan >=0) & (fade.pan <= 359)) {
acodd 1:120eeae4d43a 185 D = fade.pan - Angle; // Calculate distance to move
acodd 1:120eeae4d43a 186 } else {
acodd 1:120eeae4d43a 187 D = 0;
acodd 1:120eeae4d43a 188 abort(); // leave this function
acodd 1:120eeae4d43a 189 // add an error event handler here
acodd 1:120eeae4d43a 190 }
ms523 0:ce4a539ca1d4 191
acodd 1:120eeae4d43a 192 if (D <= 0) {
acodd 1:120eeae4d43a 193 dir = -1;
acodd 1:120eeae4d43a 194 D = abs(D);
acodd 1:120eeae4d43a 195 } else {
acodd 1:120eeae4d43a 196 dir = 1;
acodd 1:120eeae4d43a 197 }
acodd 1:120eeae4d43a 198
acodd 1:120eeae4d43a 199 if (fade.time <= (6*tsfade + 0.2)) {
acodd 1:120eeae4d43a 200 ts = tscut;
acodd 1:120eeae4d43a 201 T = fade.time;
acodd 1:120eeae4d43a 202 } else {
acodd 1:120eeae4d43a 203 ts = tsfade;
acodd 1:120eeae4d43a 204 T = fade.time;
acodd 1:120eeae4d43a 205 }
acodd 1:120eeae4d43a 206 if (fade.time <= (2*tscut+0.2)) {
acodd 1:120eeae4d43a 207 T = 6*tscut + 0.2; //min fade fime
acodd 1:120eeae4d43a 208 }
acodd 1:120eeae4d43a 209
acodd 1:120eeae4d43a 210 Vt = D / (T-(3*ts)); // Equation 1
acodd 1:120eeae4d43a 211 if (Vt > MaxSpeed) { //Check for maximum speed condition
acodd 1:120eeae4d43a 212 Vt = MaxSpeed; //Do the fade as fast as possible
acodd 1:120eeae4d43a 213 T = (D + (Vt * (3*ts)))/Vt;
acodd 1:120eeae4d43a 214 }
acodd 1:120eeae4d43a 215 // change more here
acodd 1:120eeae4d43a 216 Va =(Vt / 2); // Equation 2
acodd 1:120eeae4d43a 217
acodd 1:120eeae4d43a 218 j = ( ((2*Va)-(1.5*ts))/(3*ts*ts)); // Eqation 3
ms523 0:ce4a539ca1d4 219
acodd 1:120eeae4d43a 220 Vj = (j * ts * ts); // Equation 5
acodd 1:120eeae4d43a 221
acodd 1:120eeae4d43a 222 Vjp = Vt - Vj; // Equation 6
acodd 1:120eeae4d43a 223
acodd 1:120eeae4d43a 224 as = (Vjp-Vj)/ts; // Equation 4
acodd 1:120eeae4d43a 225
acodd 1:120eeae4d43a 226 c = Vj - (as*ts); // Equation 7
acodd 1:120eeae4d43a 227
acodd 1:120eeae4d43a 228 b = Vj - (-as*(T-ts)); // Equation 8
acodd 1:120eeae4d43a 229
acodd 1:120eeae4d43a 230 s = 0;
acodd 1:120eeae4d43a 231 fadetime = 0;
acodd 1:120eeae4d43a 232 //P = 0; //Angle;
acodd 1:120eeae4d43a 233 }
acodd 1:120eeae4d43a 234
acodd 1:120eeae4d43a 235 void BuildOut(data outdata)
acodd 1:120eeae4d43a 236 {
acodd 1:120eeae4d43a 237 htons(outdata.pan);
acodd 1:120eeae4d43a 238 htons(outdata.tilt);
acodd 1:120eeae4d43a 239 htons(outdata.time);
acodd 1:120eeae4d43a 240 htons(outdata.speed);
acodd 1:120eeae4d43a 241 htons(outdata.error);
acodd 1:120eeae4d43a 242 sprintf(outpan, "%d", outdata.pan);
acodd 1:120eeae4d43a 243 sprintf(outtilt, "%d", outdata.tilt);
acodd 1:120eeae4d43a 244 sprintf(outtime, "%d", outdata.time);
acodd 1:120eeae4d43a 245 sprintf(outspeed, "%d", outdata.speed);
acodd 1:120eeae4d43a 246 sprintf(outerror, "%d", outdata.error);
acodd 1:120eeae4d43a 247 strncpy(OutBound, header, sizeof(OutBound)-1);
acodd 1:120eeae4d43a 248 strcat(OutBound, space);
acodd 1:120eeae4d43a 249 strcat(OutBound, outpan);
acodd 1:120eeae4d43a 250 strcat(OutBound, space);
acodd 1:120eeae4d43a 251 strcat(OutBound, outtilt);
acodd 1:120eeae4d43a 252 strcat(OutBound, space);
acodd 1:120eeae4d43a 253 strcat(OutBound, outtime);
acodd 1:120eeae4d43a 254 strcat(OutBound, space);
acodd 1:120eeae4d43a 255 strcat(OutBound, outspeed);
acodd 1:120eeae4d43a 256 strcat(OutBound, space);
acodd 1:120eeae4d43a 257 strcat(OutBound, outerror);
acodd 1:120eeae4d43a 258 strcat(OutBound, space);
acodd 1:120eeae4d43a 259 }
ms523 0:ce4a539ca1d4 260
acodd 1:120eeae4d43a 261 void BreakIn()
acodd 1:120eeae4d43a 262 {
acodd 1:120eeae4d43a 263 sscanf(InBound, "%d %d %d %d %d %d %d %d %d", &checksum, &pan, &tilt, &duration, &jpan, &jtilt, &command, &newkp, &newvff);
acodd 1:120eeae4d43a 264 ntohl(checksum);
acodd 1:120eeae4d43a 265 ntohl(pan);
acodd 1:120eeae4d43a 266 ntohl(tilt);
acodd 1:120eeae4d43a 267 ntohl(duration);
acodd 1:120eeae4d43a 268 ntohl(jpan);
acodd 1:120eeae4d43a 269 ntohl(jtilt);
acodd 1:120eeae4d43a 270 ntohl(command);
acodd 1:120eeae4d43a 271 ntohl(newkp);
acodd 1:120eeae4d43a 272 ntohl(newvff);
acodd 1:120eeae4d43a 273 if (checksum == (pan + tilt)) {
acodd 1:120eeae4d43a 274 joy.pan = jpan;
acodd 1:120eeae4d43a 275 joy.tilt = jtilt;
acodd 1:120eeae4d43a 276 } else {
acodd 1:120eeae4d43a 277 command = -1; //Flag the failure
acodd 1:120eeae4d43a 278 }
acodd 1:120eeae4d43a 279 switch (command) {
acodd 1:120eeae4d43a 280 case 1: //Fade
acodd 1:120eeae4d43a 281 if (DoMove != 1) {
acodd 1:120eeae4d43a 282 fade.pan = pan;
acodd 1:120eeae4d43a 283 fade.pan = fade.pan/1000;
acodd 1:120eeae4d43a 284 fade.tilt = tilt;
acodd 1:120eeae4d43a 285 fade.tilt = fade.tilt/1000;
acodd 1:120eeae4d43a 286 fade.time = duration;
acodd 1:120eeae4d43a 287 sprintf(header, "%s", "Fade");
acodd 1:120eeae4d43a 288 Profile();
acodd 1:120eeae4d43a 289 DoMove = 1;
acodd 1:120eeae4d43a 290 } else {
acodd 1:120eeae4d43a 291 sprintf(header, "%s", "No_Moving");
acodd 1:120eeae4d43a 292 }
acodd 1:120eeae4d43a 293 break;
acodd 1:120eeae4d43a 294 case 3: // Enable
acodd 1:120eeae4d43a 295 if (Enable != 1) {
acodd 1:120eeae4d43a 296 Stopped = 0;
acodd 1:120eeae4d43a 297 P = Angle;
acodd 1:120eeae4d43a 298 Enable = 1;
acodd 1:120eeae4d43a 299 led1 = 1;
acodd 1:120eeae4d43a 300 sprintf(header, "%s", "Enable");
acodd 1:120eeae4d43a 301 } else {
acodd 1:120eeae4d43a 302 Enable = 0;
acodd 1:120eeae4d43a 303 led1 = 0;
acodd 1:120eeae4d43a 304 sprintf(header, "%s", "Disabled");
acodd 1:120eeae4d43a 305 }
acodd 1:120eeae4d43a 306 break;
acodd 1:120eeae4d43a 307 case 10: //Polling
acodd 1:120eeae4d43a 308 real.pan = (Angle * 1000);
acodd 1:120eeae4d43a 309 real.speed = (sout * 1000);
acodd 1:120eeae4d43a 310 real.error = (Error * 1000);
acodd 1:120eeae4d43a 311 //real.pan = (P * 1000);
acodd 1:120eeae4d43a 312 if ((Enable != 1) && (Stopped !=1)) {
acodd 1:120eeae4d43a 313 sprintf(header, "%s", "Disabled");
acodd 1:120eeae4d43a 314 } else if (DoMove != 0) {
acodd 1:120eeae4d43a 315 sprintf(header, "%s", "Fading");
acodd 1:120eeae4d43a 316 } else if (Stopped != 0) {
acodd 1:120eeae4d43a 317 sprintf(header, "%s", "Stopped");
acodd 1:120eeae4d43a 318 } else if (s_profile !=0) {
acodd 1:120eeae4d43a 319 sprintf(header, "%s", "Moving");
acodd 1:120eeae4d43a 320 } else {
acodd 1:120eeae4d43a 321 sprintf(header, "%s", "Idle");
acodd 1:120eeae4d43a 322 }
acodd 1:120eeae4d43a 323 break;
acodd 1:120eeae4d43a 324 case 20: // Change Values
acodd 1:120eeae4d43a 325 Kp = newkp;
acodd 1:120eeae4d43a 326 Kp = Kp/1000;
acodd 1:120eeae4d43a 327 Vff = newvff;
acodd 1:120eeae4d43a 328 Vff = Vff/1000;
acodd 1:120eeae4d43a 329 sprintf(header, "%s", "Accepted");
acodd 1:120eeae4d43a 330 break;
acodd 1:120eeae4d43a 331 case 90: //Stop
acodd 1:120eeae4d43a 332 sprintf(header, "%s", "Stop");
acodd 1:120eeae4d43a 333 Stopped = 1; //This flags that we have had a stop and must be pro-actively cleared.
acodd 1:120eeae4d43a 334 DoMove = 0; //This will stop a fade in progress machinery
acodd 1:120eeae4d43a 335 Enable = 0; //Disable the motor drives
acodd 1:120eeae4d43a 336 break;
acodd 1:120eeae4d43a 337 default:
acodd 1:120eeae4d43a 338 sprintf(header, "%s", "Error");
acodd 1:120eeae4d43a 339 }
acodd 1:120eeae4d43a 340 }
acodd 1:120eeae4d43a 341
acodd 1:120eeae4d43a 342 void Comms_thread(void const *args)
acodd 1:120eeae4d43a 343 {
acodd 1:120eeae4d43a 344 while(true) {
acodd 1:120eeae4d43a 345 if (client.receive(InBound, sizeof(InBound)-1) >=0) {
acodd 1:120eeae4d43a 346 BreakIn();
acodd 1:120eeae4d43a 347 BuildOut(real);
acodd 1:120eeae4d43a 348 client.send(OutBound, sizeof(OutBound));
acodd 1:120eeae4d43a 349 }
acodd 1:120eeae4d43a 350 Thread::wait(8);
acodd 1:120eeae4d43a 351 }
acodd 1:120eeae4d43a 352 }
acodd 1:120eeae4d43a 353
acodd 1:120eeae4d43a 354 void LCD_thread(void const *args)
acodd 1:120eeae4d43a 355 {
acodd 1:120eeae4d43a 356 while (true) {
acodd 1:120eeae4d43a 357 lcd.cls();
acodd 1:120eeae4d43a 358 pc.printf("A=%7.4f, P= %f, D= %f, T= %f,\r",Angle,P,D,T);
acodd 1:120eeae4d43a 359 //lcd.printf("%3.2f,%3.2f,%3.2f", Angle,P,demand);
acodd 1:120eeae4d43a 360 lcd.locate(0,0);
acodd 1:120eeae4d43a 361 lcd.printf("A=%f", Angle);
acodd 1:120eeae4d43a 362 //lcd.printf("J=%f", joy.pan);
acodd 1:120eeae4d43a 363 lcd.locate(0,1);
acodd 1:120eeae4d43a 364 lcd.printf("s=%f", s_profile);
acodd 1:120eeae4d43a 365 Thread::wait(32);
acodd 1:120eeae4d43a 366 }
acodd 1:120eeae4d43a 367 }
acodd 1:120eeae4d43a 368
acodd 1:120eeae4d43a 369 void DriveMotor(float speed)
acodd 1:120eeae4d43a 370 {
acodd 1:120eeae4d43a 371 Aout = (speed/MaxSpeed) + AOffset;
acodd 1:120eeae4d43a 372 Cop = Aout;
acodd 1:120eeae4d43a 373 }
acodd 1:120eeae4d43a 374
acodd 1:120eeae4d43a 375 void ReadEncoder()
acodd 1:120eeae4d43a 376 {
acodd 1:120eeae4d43a 377 High = spi.write(0x0000); //Write dummy value to read 16 MSB
acodd 1:120eeae4d43a 378 Low = spi.write(0x0000); //Write dummy value to read 16 LSB
acodd 1:120eeae4d43a 379 Encoder=(High<<16)+Low;
acodd 1:120eeae4d43a 380 Encoder=(Encoder & 0x1FFFFF80);
acodd 1:120eeae4d43a 381 Encoder=(Encoder>>7);
acodd 1:120eeae4d43a 382 Angle = (Encoder * 0.0000858306885);
acodd 1:120eeae4d43a 383 Angle = Angle + Offset;
acodd 1:120eeae4d43a 384 if (Angle < 0) {
acodd 1:120eeae4d43a 385 Angle = Angle + 360;
acodd 1:120eeae4d43a 386 }
acodd 1:120eeae4d43a 387 }
acodd 1:120eeae4d43a 388 void ZeroEncoder()
acodd 1:120eeae4d43a 389 {
acodd 1:120eeae4d43a 390 High = spi.write(0x0000); //Write dummy value to read 16 MSB
acodd 1:120eeae4d43a 391 Low = spi.write(0x0000); //Write dummy value to read 16 LSB
acodd 1:120eeae4d43a 392 Encoder=(High<<16)+Low;
acodd 1:120eeae4d43a 393 Encoder=(Encoder & 0x1FFFFF80);
acodd 1:120eeae4d43a 394 Encoder=(Encoder>>7);
acodd 1:120eeae4d43a 395 Angle = Encoder * 0.0000858306885;
acodd 1:120eeae4d43a 396 Offset = (Angle * -1)+180;
acodd 1:120eeae4d43a 397 pc.printf("Zero conplete %7.4f, ,%7.4f",Angle, Offset);
acodd 1:120eeae4d43a 398 Angle = Angle + Offset;
acodd 1:120eeae4d43a 399 if (Angle < 0) {
acodd 1:120eeae4d43a 400 Angle = Angle + 360;
acodd 1:120eeae4d43a 401 }
acodd 1:120eeae4d43a 402 pc.printf(" New A=%7.4f ",Angle);
acodd 1:120eeae4d43a 403 P = Angle;
acodd 1:120eeae4d43a 404 wait(0.1);
ms523 0:ce4a539ca1d4 405 }
ms523 0:ce4a539ca1d4 406
ms523 0:ce4a539ca1d4 407
acodd 1:120eeae4d43a 408 void Servo_Trap(void const *args) //This is threaded with real-time priority.
acodd 1:120eeae4d43a 409 {
acodd 1:120eeae4d43a 410 while(true) {
acodd 1:120eeae4d43a 411 ReadEncoder();
acodd 1:120eeae4d43a 412 if ((DoMove == 1) & (Enable != 0) & (Stopped !=1)) {
acodd 1:120eeae4d43a 413 if ((fadetime < ta) & (s < Vt)) {
acodd 1:120eeae4d43a 414 led2 = 1;
acodd 1:120eeae4d43a 415 s = s + Vs;
acodd 1:120eeae4d43a 416 fadetime = fadetime + 0.005; // This provides the base time for the fade sequence
acodd 1:120eeae4d43a 417 } else if ((fadetime >= ta) & (fadetime <(T-ta))) {
acodd 1:120eeae4d43a 418 s = Vt;
acodd 1:120eeae4d43a 419 fadetime = fadetime + 0.005;
acodd 1:120eeae4d43a 420 } else if ((fadetime >= (T-ta)) & (fadetime < T)) {
acodd 1:120eeae4d43a 421 s = s-Vs;
acodd 1:120eeae4d43a 422 fadetime = fadetime + 0.005;
acodd 1:120eeae4d43a 423 } else if (fadetime >= T) {
acodd 1:120eeae4d43a 424 s=0;
acodd 1:120eeae4d43a 425 led2 = 0;
acodd 1:120eeae4d43a 426 DoMove = 0;
acodd 1:120eeae4d43a 427 } else {
acodd 1:120eeae4d43a 428 fadetime = fadetime + 0.005; // for TBC reason this is needed!
acodd 1:120eeae4d43a 429 }
acodd 1:120eeae4d43a 430 // compute the new position demand:
acodd 1:120eeae4d43a 431 s_profile = s * dir;
acodd 1:120eeae4d43a 432 P = P + (s_profile * 0.005);
acodd 1:120eeae4d43a 433 real.time = ((T - fadetime) * 1000);
acodd 1:120eeae4d43a 434 } else {
acodd 1:120eeae4d43a 435 DoMove = 0;
acodd 1:120eeae4d43a 436 real.time = 0;
acodd 1:120eeae4d43a 437 } // end of DoMove (Fade or Cut
acodd 1:120eeae4d43a 438
acodd 1:120eeae4d43a 439 if ((Enable !=0) & (Stopped !=1)) { //This executes the core postion loop.
acodd 1:120eeae4d43a 440 if (DoMove == 0) { //Adding Joystick Demand as appropriate
acodd 1:120eeae4d43a 441 s_profile = MaxSpeed * (joy.pan/100);
acodd 1:120eeae4d43a 442 P = P + (s_profile * 0.005);
acodd 1:120eeae4d43a 443 } else {
acodd 1:120eeae4d43a 444 joy.pan = 0;
acodd 1:120eeae4d43a 445 joy.tilt = 0;
acodd 1:120eeae4d43a 446 }
acodd 1:120eeae4d43a 447 Error = (P - Angle);
acodd 1:120eeae4d43a 448 Prop = Kp * Error;
acodd 1:120eeae4d43a 449 demand = s_profile+Prop;
acodd 1:120eeae4d43a 450 DriveMotor(demand);
acodd 1:120eeae4d43a 451 } else {
acodd 1:120eeae4d43a 452 DriveMotor(0);
acodd 1:120eeae4d43a 453 } //End of core position loop
acodd 1:120eeae4d43a 454 Thread::wait(5);
ms523 0:ce4a539ca1d4 455 }
ms523 0:ce4a539ca1d4 456 }
ms523 0:ce4a539ca1d4 457
acodd 1:120eeae4d43a 458 void Servo(void const *args) //This is threaded with real-time priority.
acodd 1:120eeae4d43a 459 // This is S ramped servo control based on the S ramp Profile calculations
acodd 1:120eeae4d43a 460 {
acodd 1:120eeae4d43a 461 while(true) {
acodd 1:120eeae4d43a 462 ReadEncoder();
acodd 1:120eeae4d43a 463 if ((DoMove == 1) & (Enable != 0) & (Stopped !=1)) {
acodd 1:120eeae4d43a 464 if ((fadetime < ts) & (s < Vt)) {
acodd 1:120eeae4d43a 465 led2 = 0;
acodd 1:120eeae4d43a 466 s = j*fadetime*fadetime; //bottom parabola
acodd 1:120eeae4d43a 467 fadetime = fadetime + 0.005; // This provides the base time for the fade sequence
acodd 1:120eeae4d43a 468 } else if ((fadetime >= ts) & (fadetime <(2*ts))) {
acodd 1:120eeae4d43a 469 s = (as*fadetime)+c; //steady accel stage
acodd 1:120eeae4d43a 470 fadetime = fadetime + 0.005;
acodd 1:120eeae4d43a 471 } else if ((fadetime >= (2*ts)) & (fadetime <(3*ts))) {
acodd 1:120eeae4d43a 472 s = (-j*(fadetime-(3*ts))*(fadetime-(3*ts))) + Vt; // Top parabola
acodd 1:120eeae4d43a 473 fadetime = fadetime + 0.005;
acodd 1:120eeae4d43a 474 } else if ((fadetime >= (3*ts)) & (fadetime <(T-(3*ts)))) {
acodd 1:120eeae4d43a 475 s = Vt; // Steady Speed Stage
acodd 1:120eeae4d43a 476 fadetime = fadetime + 0.005;
acodd 1:120eeae4d43a 477 } else if ((fadetime >= (T-(3*ts))) & (fadetime <(T-(2*ts)))) {
acodd 1:120eeae4d43a 478 s = (-j*(fadetime-(T-(3*ts)))*(fadetime-(T-(3*ts)))) + Vt; // Top parabola down
acodd 1:120eeae4d43a 479 fadetime = fadetime + 0.005;
acodd 1:120eeae4d43a 480 } else if ((fadetime >= (T-ts-ts)) & (fadetime < (T-ts))) {
acodd 1:120eeae4d43a 481 s = (-as*fadetime)+b; //steady decel stage
acodd 1:120eeae4d43a 482 fadetime = fadetime + 0.005;
acodd 1:120eeae4d43a 483 } else if ((fadetime >= (T-ts)) & (s < Vt) & (fadetime <= T)) {
acodd 1:120eeae4d43a 484 led2 = 1;
acodd 1:120eeae4d43a 485 s = j*(T-fadetime)*(T-fadetime); //bottom parabola to end
acodd 1:120eeae4d43a 486 fadetime = fadetime + 0.005;
acodd 1:120eeae4d43a 487 } else if (fadetime >= T) {
acodd 1:120eeae4d43a 488 s=0;
acodd 1:120eeae4d43a 489 led2 = 0;
acodd 1:120eeae4d43a 490 DoMove = 0;
acodd 1:120eeae4d43a 491 } else {
acodd 1:120eeae4d43a 492 fadetime = fadetime + 0.005; // for TBC reason this is needed!
acodd 1:120eeae4d43a 493 }
acodd 1:120eeae4d43a 494 // compute the new position demand:
acodd 1:120eeae4d43a 495 s_profile = s * dir;
acodd 1:120eeae4d43a 496 P = P + (s_profile * 0.005);
acodd 1:120eeae4d43a 497 real.time = ((T - fadetime) * 1000);
acodd 1:120eeae4d43a 498 } else {
acodd 1:120eeae4d43a 499 DoMove = 0;
acodd 1:120eeae4d43a 500 real.time = 0;
acodd 1:120eeae4d43a 501 } // end of DoMove (Fade or Cut
acodd 1:120eeae4d43a 502
acodd 1:120eeae4d43a 503 if ((Enable !=0) & (Stopped !=1)) { //This executes the core postion loop.
acodd 1:120eeae4d43a 504 if (DoMove == 0) { //Adding Joystick Demand as appropriate
acodd 1:120eeae4d43a 505 s_profile = MaxSpeed * (joy.pan/100);
acodd 1:120eeae4d43a 506 P = P + (s_profile * 0.005);
acodd 1:120eeae4d43a 507 } else {
acodd 1:120eeae4d43a 508 joy.pan = 0;
acodd 1:120eeae4d43a 509 joy.tilt = 0;
acodd 1:120eeae4d43a 510 }
acodd 1:120eeae4d43a 511 sout = s_profile * Vff; //Apply velocity feedforward term
acodd 1:120eeae4d43a 512 Error = (P - Angle); // Position Error
acodd 1:120eeae4d43a 513 Prop = Kp * Error; // Calculate proportional gain element
acodd 1:120eeae4d43a 514 demand = sout + Prop; // Sum the result of Vff and Kp to the demand
acodd 1:120eeae4d43a 515 DriveMotor(demand); // Send the demand to the motor
acodd 1:120eeae4d43a 516 } else {
acodd 1:120eeae4d43a 517 DriveMotor(0);
acodd 1:120eeae4d43a 518 } //End of core position loop
acodd 1:120eeae4d43a 519 Thread::wait(5);
acodd 1:120eeae4d43a 520 }
acodd 1:120eeae4d43a 521 }
acodd 1:120eeae4d43a 522
acodd 1:120eeae4d43a 523
acodd 1:120eeae4d43a 524 int main()
acodd 1:120eeae4d43a 525 {
acodd 1:120eeae4d43a 526
acodd 1:120eeae4d43a 527 Enable = 0; // Try to prevent movement on startup
acodd 1:120eeae4d43a 528 DriveMotor(0); // Drive demand to Zero
acodd 1:120eeae4d43a 529 lcd.cls();
acodd 1:120eeae4d43a 530 lcd.printf("2.5: Connecting");
acodd 1:120eeae4d43a 531
acodd 1:120eeae4d43a 532
acodd 1:120eeae4d43a 533 pc.baud(115200); //Pimp the baud rate
acodd 1:120eeae4d43a 534
acodd 1:120eeae4d43a 535 spi.format(16,2); // Setup the spi for 14 bit data, high steady state clock, read on rising edge
acodd 1:120eeae4d43a 536 spi.frequency(400000); //200kHz
acodd 1:120eeae4d43a 537 ReadEncoder();
acodd 1:120eeae4d43a 538 pc.printf("1st Read says %7.4f ",Angle);
acodd 1:120eeae4d43a 539 ZeroEncoder();
acodd 1:120eeae4d43a 540
acodd 1:120eeae4d43a 541 ReadEncoder();
acodd 1:120eeae4d43a 542 pc.printf("Read says %7.4f ",Angle);
acodd 1:120eeae4d43a 543
acodd 1:120eeae4d43a 544 Connectme(); //Connect to the outside world
acodd 1:120eeae4d43a 545
acodd 1:120eeae4d43a 546
acodd 1:120eeae4d43a 547 Thread thread_COMMS(Comms_thread); //Start reading from the network.
acodd 1:120eeae4d43a 548 Thread thread_LCD(LCD_thread); //Start updating the LCD
acodd 1:120eeae4d43a 549
acodd 1:120eeae4d43a 550 Thread thread_Servo(Servo);
acodd 1:120eeae4d43a 551 thread_Servo.set_priority(osPriorityRealtime);
acodd 1:120eeae4d43a 552
acodd 1:120eeae4d43a 553 //Profile();
acodd 1:120eeae4d43a 554 wait(2);
acodd 1:120eeae4d43a 555
acodd 1:120eeae4d43a 556 while(1) {
acodd 1:120eeae4d43a 557 // if (Run == 1) {
acodd 1:120eeae4d43a 558 // wait(0.01); //de-bounce
acodd 1:120eeae4d43a 559 // if (Run == 1) {
acodd 1:120eeae4d43a 560
acodd 1:120eeae4d43a 561 // DoMove = 1;
acodd 1:120eeae4d43a 562 // }
acodd 1:120eeae4d43a 563 // }
acodd 1:120eeae4d43a 564 }
acodd 1:120eeae4d43a 565 }
acodd 1:120eeae4d43a 566 /*
acodd 1:120eeae4d43a 567 FILE *fp = fopen("/local/log.txt", "a"); // Open "log.txt" on the local file system for append
acodd 1:120eeae4d43a 568 fprintf(fp,"Angle %f\r\n",Angle); //
acodd 1:120eeae4d43a 569 fclose(fp);
acodd 1:120eeae4d43a 570 wait(2);
acodd 1:120eeae4d43a 571 ReadEncoder();
acodd 1:120eeae4d43a 572 fopen("/local/log.txt", "a"); // Open "log.txt" on the local file system for append
acodd 1:120eeae4d43a 573 fprintf(fp,"Angle %f\r\n",Angle); //
acodd 1:120eeae4d43a 574 fclose(fp);
acodd 1:120eeae4d43a 575 void ReadFile (void) {
acodd 1:120eeae4d43a 576 FILE *set = fopen("/local/setup.txt", "r"); // Open "setup.txt" on the local file system for read
acodd 1:120eeae4d43a 577 fscanf(set,"%s %d",A,&offset); // read offset
acodd 1:120eeae4d43a 578 fscanf(set,"%s %f",B,&gain); // read gain
acodd 1:120eeae4d43a 579 fclose(set);
acodd 1:120eeae4d43a 580 }
acodd 1:120eeae4d43a 581 void WriteFile(void) { // write to USB memory
acodd 1:120eeae4d43a 582 FILE *fp = fopen("/local/setup.txt", "a"); // Open "setup.txt" on the local file system for append
acodd 1:120eeae4d43a 583 fprintf(fp,"offset %d\r\n",Angle); //
acodd 1:120eeae4d43a 584 fprintf(fp,"gain %f\r\n",s); //
acodd 1:120eeae4d43a 585 fclose(fp);
acodd 1:120eeae4d43a 586 }
acodd 1:120eeae4d43a 587 */
acodd 1:120eeae4d43a 588