Vitec Group / Mbed 2 deprecated 1_System_Controller

Dependencies:   EthernetInterface TextLCD mbed-rtos mbed

Fork of 1_System_Controller by Andrew Codd

main.cpp@2:4173f8896073, 2015-04-15 (annotated)

Committer:
acodd
Date:
Wed Apr 15 15:43:16 2015 +0000
Revision:
2:4173f8896073
Parent:
1:120eeae4d43a 
Updated 15/04/2015

Who changed what in which revision?

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