MorphGI / Mbed OS MGI_Rebuild_MidLevel_9_0-Basic_PWM

Basic Mid-Level control for the rebuilt MorphGI control unit, using PWM to communicate with the low level controllers.

Dependencies:   ros_lib_kinetic

main.cpp@7:5b6a2cefbf3b, 2018-08-03 (annotated)

Committer:
WD40andTape
Date:
Fri Aug 03 14:58:36 2018 +0000
Revision:
7:5b6a2cefbf3b
Parent:
6:f0a18e28a322
Child:
8:d6657767a182
Moved HL comms into a separate class/file.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WD40andTape 7:5b6a2cefbf3b 1 // STANDARD IMPORTS
WD40andTape 7:5b6a2cefbf3b 2 #include "math.h"
WD40andTape 7:5b6a2cefbf3b 3 // MBED IMPORTS
dofydoink 0:607bc887b6e0 4 #include "mbed.h"
WD40andTape 6:f0a18e28a322 5 #include "mbed_events.h"
WD40andTape 7:5b6a2cefbf3b 6 // CUSTOM IMPORTS
WD40andTape 7:5b6a2cefbf3b 7 #include "HLComms.h"
WD40andTape 6:f0a18e28a322 8
WD40andTape 7:5b6a2cefbf3b 9 // ADC SPI DEFINES
dofydoink 0:607bc887b6e0 10 #define PREAMBLE 0x06
dofydoink 0:607bc887b6e0 11 #define CHAN_1 0x30
dofydoink 0:607bc887b6e0 12 #define CHAN_2 0x70
dofydoink 0:607bc887b6e0 13 #define CHAN_3 0xB0
dofydoink 0:607bc887b6e0 14 #define CHAN_4 0xF0
dofydoink 0:607bc887b6e0 15 #define DATA_MASK 0x0F
dofydoink 0:607bc887b6e0 16
WD40andTape 7:5b6a2cefbf3b 17 // SIMPLE CHANNEL SELECTION
dofydoink 0:607bc887b6e0 18 #define ADC_PRESSURE 1
dofydoink 0:607bc887b6e0 19 #define ADC_POSITION 3
WD40andTape 7:5b6a2cefbf3b 20
WD40andTape 7:5b6a2cefbf3b 21 #define N_CHANNELS 8 // Number of channels to control
WD40andTape 7:5b6a2cefbf3b 22 // 1-3: front segment; 4-6: rear segment; 7-8: mid segment
dofydoink 0:607bc887b6e0 23
WD40andTape 7:5b6a2cefbf3b 24 // SPI SETTINGS
dofydoink 0:607bc887b6e0 25 #define LOW_LEVEL_SPI_FREQUENCY 10000000
WD40andTape 7:5b6a2cefbf3b 26 // PATH GENERATION SETTINGS
WD40andTape 6:f0a18e28a322 27 #define PATH_SAMPLE_TIME_S 0.005 //0.109
dofydoink 0:607bc887b6e0 28 #define MAX_LENGTH_MM 100.0
WD40andTape 3:c83291bf9fd2 29 #define MAX_ACTUATOR_LENGTH 52.2
dofydoink 0:607bc887b6e0 30 #define MAX_SPEED_MMPS 24.3457
WD40andTape 7:5b6a2cefbf3b 31 #define PATH_TOLERANCE_MM 0.2 // How close the linear path must get to the target position before considering it a success.
WD40andTape 7:5b6a2cefbf3b 32
WD40andTape 7:5b6a2cefbf3b 33 #define BAUD_RATE 9600 //115200
WD40andTape 7:5b6a2cefbf3b 34
WD40andTape 7:5b6a2cefbf3b 35 // COMMS SETTINGS
WD40andTape 7:5b6a2cefbf3b 36 const short int SERVER_PORT = 80;
dofydoink 0:607bc887b6e0 37
WD40andTape 6:f0a18e28a322 38 //const double DBL_MAX_CHAMBER_LENGTHS_MM[N_CHANNELS] = {80.0,80.0,80.0,80.0,80.0,80.0,80.0,80.0};
WD40andTape 7:5b6a2cefbf3b 39 //const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {0.30586,0.30586,0.30586,0.30586,0.30586,0.4588,0.4588}; // Convert from chamber lengths to actuator lengths
WD40andTape 7:5b6a2cefbf3b 40 const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; // Convert from chamber lengths to actuator
WD40andTape 4:303584310071 41 const double DBL_SMOOTHING_FACTOR = 0.5; // 0<x<=1, where 1 is no smoothing
WD40andTape 7:5b6a2cefbf3b 42 const double DBL_PATH_TOLERANCE = MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; // Path tolerance in mm with 5% tolerance
dofydoink 0:607bc887b6e0 43
WD40andTape 7:5b6a2cefbf3b 44 // PATH VARIABLES
WD40andTape 7:5b6a2cefbf3b 45 double dblVelocity_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 46 double dblLinearPathCurrentPos_mm[N_CHANNELS]={ 0.0 }; // The current position of the linear path (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 47 double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
dofydoink 0:607bc887b6e0 48
WD40andTape 7:5b6a2cefbf3b 49 int intDemandPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator
WD40andTape 7:5b6a2cefbf3b 50 char chrErrorFlag[N_CHANNELS];// 3 error bits from LL
dofydoink 0:607bc887b6e0 51
WD40andTape 7:5b6a2cefbf3b 52 // PIN DECLARATIONS
WD40andTape 7:5b6a2cefbf3b 53 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
WD40andTape 3:c83291bf9fd2 54 InterruptIn pinGate6(PE_11); //this pin HAS TO BE defined before SPI set up. No Clue Why.
WD40andTape 3:c83291bf9fd2 55 SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
WD40andTape 3:c83291bf9fd2 56 DigitalOut cs_LL[N_CHANNELS] = {PD_15, PE_10, PD_14, PD_11, PE_7, PD_12, PF_10, PD_13};//chip select for low level controller
WD40andTape 3:c83291bf9fd2 57 DigitalOut cs_ADC[N_CHANNELS] = {PG_12, PG_9, PE_1, PG_0, PD_0, PD_1, PF_0, PF_1}; //chip select for ADC
WD40andTape 3:c83291bf9fd2 58 DigitalOut pinCheck(PE_5);
WD40andTape 7:5b6a2cefbf3b 59 // These interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
WD40andTape 3:c83291bf9fd2 60 InterruptIn pinGate0(PF_11);
WD40andTape 3:c83291bf9fd2 61 InterruptIn pinGate1(PG_14);
WD40andTape 3:c83291bf9fd2 62 InterruptIn pinGate2(PF_15);
WD40andTape 3:c83291bf9fd2 63 InterruptIn pinGate3(PF_12);
WD40andTape 3:c83291bf9fd2 64 InterruptIn pinGate4(PF_3);
WD40andTape 3:c83291bf9fd2 65 InterruptIn pinGate5(PF_13);
WD40andTape 7:5b6a2cefbf3b 66 //InterruptIn pinGate6(PE_11); // See above nonsense
WD40andTape 3:c83291bf9fd2 67 InterruptIn pinGate7(PE_13);
WD40andTape 7:5b6a2cefbf3b 68 DigitalOut pinReset(PD_2); // Reset pin for all controllers.
WD40andTape 3:c83291bf9fd2 69
dofydoink 0:607bc887b6e0 70 EventQueue queue(32 * EVENTS_EVENT_SIZE);
dofydoink 0:607bc887b6e0 71 Thread t(osPriorityRealtime);
WD40andTape 4:303584310071 72 Thread threadReceiveAndReplan(osPriorityBelowNormal);
WD40andTape 4:303584310071 73 Thread threadSmoothPathPlan(osPriorityNormal);
dofydoink 0:607bc887b6e0 74 Thread threadSimulateDemand(osPriorityHigh);
dofydoink 0:607bc887b6e0 75
WD40andTape 1:2a43cf183a62 76 Mutex mutChannel[N_CHANNELS];
WD40andTape 4:303584310071 77 Mutex mutPathIn;
WD40andTape 7:5b6a2cefbf3b 78 Semaphore semPathPlan(1);
dofydoink 0:607bc887b6e0 79
WD40andTape 7:5b6a2cefbf3b 80 Timer timer;
WD40andTape 7:5b6a2cefbf3b 81 Ticker PathCalculationTicker;
dofydoink 0:607bc887b6e0 82
dofydoink 0:607bc887b6e0 83 int ThreadID[N_CHANNELS];
dofydoink 0:607bc887b6e0 84
WD40andTape 1:2a43cf183a62 85 bool isDataReady[N_CHANNELS]; // flag to indicate path data is ready for transmission to low level.
dofydoink 0:607bc887b6e0 86
dofydoink 5:712e7634c779 87 double dblGlobalTest;
dofydoink 5:712e7634c779 88 int intGlobalTest;
WD40andTape 7:5b6a2cefbf3b 89
WD40andTape 7:5b6a2cefbf3b 90 double ReadADCPosition_mtrs(int channel) {
dofydoink 0:607bc887b6e0 91 unsigned int outputA;
dofydoink 0:607bc887b6e0 92 unsigned int outputB;
WD40andTape 3:c83291bf9fd2 93 int output;
WD40andTape 3:c83291bf9fd2 94 double dblOutput;
dofydoink 0:607bc887b6e0 95
dofydoink 0:607bc887b6e0 96 spi.format(8,0);
dofydoink 0:607bc887b6e0 97 spi.frequency(1000000);
dofydoink 0:607bc887b6e0 98
dofydoink 0:607bc887b6e0 99 cs_ADC[channel] = 0;
dofydoink 0:607bc887b6e0 100 spi.write(PREAMBLE);
dofydoink 0:607bc887b6e0 101 outputA = spi.write(CHAN_3);
dofydoink 0:607bc887b6e0 102 outputB = spi.write(0xFF);
WD40andTape 1:2a43cf183a62 103 cs_ADC[channel]= 1;
dofydoink 0:607bc887b6e0 104
dofydoink 0:607bc887b6e0 105 outputA = outputA & DATA_MASK;
dofydoink 0:607bc887b6e0 106 outputA = outputA<<8;
dofydoink 0:607bc887b6e0 107 output = (outputA | outputB);
WD40andTape 3:c83291bf9fd2 108 output = 4095- output;
dofydoink 5:712e7634c779 109 dblOutput = (double) (output);
dofydoink 5:712e7634c779 110 dblOutput = dblOutput*0.0229 - 21.582;
WD40andTape 3:c83291bf9fd2 111 return dblOutput;
dofydoink 0:607bc887b6e0 112 }
dofydoink 0:607bc887b6e0 113
WD40andTape 7:5b6a2cefbf3b 114 double ReadADCPressure_bar(int channel) {
dofydoink 0:607bc887b6e0 115 unsigned int outputA;
dofydoink 0:607bc887b6e0 116 unsigned int outputB;
WD40andTape 3:c83291bf9fd2 117 int output;
WD40andTape 3:c83291bf9fd2 118 double dblOutput;
dofydoink 0:607bc887b6e0 119
dofydoink 0:607bc887b6e0 120 spi.format(8,0);
dofydoink 0:607bc887b6e0 121 spi.frequency(1000000);
dofydoink 0:607bc887b6e0 122
dofydoink 0:607bc887b6e0 123 cs_ADC[channel] = 0;
dofydoink 0:607bc887b6e0 124 spi.write(PREAMBLE);
dofydoink 0:607bc887b6e0 125 outputA = spi.write(CHAN_1);
dofydoink 0:607bc887b6e0 126 outputB = spi.write(0xFF);
dofydoink 0:607bc887b6e0 127 cs_ADC[channel] = 1;
dofydoink 0:607bc887b6e0 128
dofydoink 0:607bc887b6e0 129 outputA = outputA & DATA_MASK;
dofydoink 0:607bc887b6e0 130 outputA = outputA<<8;
dofydoink 0:607bc887b6e0 131 output = (outputA | outputB);
dofydoink 0:607bc887b6e0 132
dofydoink 5:712e7634c779 133 dblOutput = (double)(output);
dofydoink 5:712e7634c779 134 dblOutput = dblOutput-502.0;
dofydoink 5:712e7634c779 135 dblOutput = dblOutput/4095.0*8.0;
WD40andTape 3:c83291bf9fd2 136 return dblOutput;
dofydoink 0:607bc887b6e0 137 }
dofydoink 0:607bc887b6e0 138
WD40andTape 7:5b6a2cefbf3b 139 void SendReceiveData(int channel) {
WD40andTape 7:5b6a2cefbf3b 140 int intPosSPI_Rx[N_CHANNELS]; // 13 bit value received over SPI from the actuator
WD40andTape 4:303584310071 141
WD40andTape 7:5b6a2cefbf3b 142 // Get data from controller
dofydoink 0:607bc887b6e0 143 spi.format(16,2);
dofydoink 0:607bc887b6e0 144 spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 7:5b6a2cefbf3b 145 mutChannel[channel].lock(); // Lock mutex for specific Channel
WD40andTape 7:5b6a2cefbf3b 146 cs_LL[channel] = 0; // Select relevant chip
WD40andTape 7:5b6a2cefbf3b 147 intPosSPI_Rx[channel] = spi.write(intDemandPos_Tx[channel]); // Transmit & receive
WD40andTape 7:5b6a2cefbf3b 148 cs_LL[channel] = 1; // Deselect chip
WD40andTape 7:5b6a2cefbf3b 149 isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
dofydoink 5:712e7634c779 150 if(channel == 0) {
dofydoink 5:712e7634c779 151 intGlobalTest = intPosSPI_Rx[channel];
dofydoink 5:712e7634c779 152 dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
dofydoink 5:712e7634c779 153 }
WD40andTape 1:2a43cf183a62 154
WD40andTape 7:5b6a2cefbf3b 155 // Sort out received data
dofydoink 0:607bc887b6e0 156 chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
WD40andTape 4:303584310071 157
dofydoink 0:607bc887b6e0 158 intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
WD40andTape 4:303584310071 159 //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
WD40andTape 4:303584310071 160 mutChannel[channel].unlock();//unlock mutex for specific channel
WD40andTape 3:c83291bf9fd2 161 //printf("%d, %d\r\n",intPosSPI_Rx[0],intDemandPos_Tx[0]);
WD40andTape 3:c83291bf9fd2 162 }
WD40andTape 3:c83291bf9fd2 163
WD40andTape 7:5b6a2cefbf3b 164 // Common rise handler function
WD40andTape 7:5b6a2cefbf3b 165 void common_rise_handler(int channel) {
WD40andTape 3:c83291bf9fd2 166 pinCheck = 1;
WD40andTape 7:5b6a2cefbf3b 167 if (isDataReady[channel]) { // Check if data is ready for tranmission
WD40andTape 7:5b6a2cefbf3b 168 ThreadID[channel] = queue.call(SendReceiveData,channel); // Schedule transmission
dofydoink 0:607bc887b6e0 169 }
dofydoink 0:607bc887b6e0 170 }
dofydoink 0:607bc887b6e0 171
WD40andTape 7:5b6a2cefbf3b 172 // Common fall handler functions
WD40andTape 7:5b6a2cefbf3b 173 void common_fall_handler(int channel) {
WD40andTape 3:c83291bf9fd2 174 pinCheck = 0;
WD40andTape 7:5b6a2cefbf3b 175 queue.cancel(ThreadID[channel]); // Cancel relevant queued event
dofydoink 0:607bc887b6e0 176 }
dofydoink 0:607bc887b6e0 177
WD40andTape 7:5b6a2cefbf3b 178 // Stub rise functions
dofydoink 0:607bc887b6e0 179 void rise0(void) { common_rise_handler(0); }
dofydoink 0:607bc887b6e0 180 void rise1(void) { common_rise_handler(1); }
dofydoink 0:607bc887b6e0 181 void rise2(void) { common_rise_handler(2); }
dofydoink 0:607bc887b6e0 182 void rise3(void) { common_rise_handler(3); }
dofydoink 0:607bc887b6e0 183 void rise4(void) { common_rise_handler(4); }
dofydoink 0:607bc887b6e0 184 void rise5(void) { common_rise_handler(5); }
dofydoink 0:607bc887b6e0 185 void rise6(void) { common_rise_handler(6); }
dofydoink 0:607bc887b6e0 186 void rise7(void) { common_rise_handler(7); }
dofydoink 0:607bc887b6e0 187
WD40andTape 7:5b6a2cefbf3b 188 // Stub fall functions
dofydoink 0:607bc887b6e0 189 void fall0(void) { common_fall_handler(0); }
dofydoink 0:607bc887b6e0 190 void fall1(void) { common_fall_handler(1); }
dofydoink 0:607bc887b6e0 191 void fall2(void) { common_fall_handler(2); }
dofydoink 0:607bc887b6e0 192 void fall3(void) { common_fall_handler(3); }
dofydoink 0:607bc887b6e0 193 void fall4(void) { common_fall_handler(4); }
dofydoink 0:607bc887b6e0 194 void fall5(void) { common_fall_handler(5); }
dofydoink 0:607bc887b6e0 195 void fall6(void) { common_fall_handler(6); }
dofydoink 0:607bc887b6e0 196 void fall7(void) { common_fall_handler(7); }
dofydoink 0:607bc887b6e0 197
WD40andTape 7:5b6a2cefbf3b 198 void startPathPlan() { // Plan a new linear path after receiving new target data
WD40andTape 4:303584310071 199 semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
dofydoink 0:607bc887b6e0 200 }
dofydoink 0:607bc887b6e0 201
WD40andTape 7:5b6a2cefbf3b 202 // This function will be called when a new transmission is received from high level
WD40andTape 7:5b6a2cefbf3b 203 void ReceiveAndReplan() {
WD40andTape 7:5b6a2cefbf3b 204 HLComms hlcomms(SERVER_PORT);
WD40andTape 6:f0a18e28a322 205
WD40andTape 6:f0a18e28a322 206 int error_code;
WD40andTape 7:5b6a2cefbf3b 207 error_code = hlcomms.setup_server();
WD40andTape 6:f0a18e28a322 208 if( error_code == -1 ) return;
WD40andTape 7:5b6a2cefbf3b 209 error_code = hlcomms.accept_connection();
WD40andTape 7:5b6a2cefbf3b 210 if( error_code == -1 ) {
WD40andTape 7:5b6a2cefbf3b 211 hlcomms.close_server();
WD40andTape 7:5b6a2cefbf3b 212 return;
WD40andTape 7:5b6a2cefbf3b 213 }
WD40andTape 6:f0a18e28a322 214
WD40andTape 7:5b6a2cefbf3b 215 int ii;
WD40andTape 6:f0a18e28a322 216 unsigned char recv_buffer[1024];
WD40andTape 7:5b6a2cefbf3b 217 struct msg_format input; //hlcomms.msg_format
WD40andTape 6:f0a18e28a322 218 char send_buffer[65];
WD40andTape 6:f0a18e28a322 219 char * message_to_send;
WD40andTape 4:303584310071 220
WD40andTape 6:f0a18e28a322 221 while( true ) {
WD40andTape 6:f0a18e28a322 222 // RECEIVE MESSAGE
WD40andTape 6:f0a18e28a322 223 memset(recv_buffer, 0, sizeof(recv_buffer));
WD40andTape 7:5b6a2cefbf3b 224 error_code = hlcomms.interfaces.clt_sock.recv(recv_buffer, 1024); // Blocks until data is received
WD40andTape 6:f0a18e28a322 225 if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
WD40andTape 6:f0a18e28a322 226 printf("Client disconnected.\n\r");
WD40andTape 7:5b6a2cefbf3b 227 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 228 return;
WD40andTape 6:f0a18e28a322 229 } else if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 230 printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 231 "Perhaps the server socket is not connected to a remote host? "
WD40andTape 6:f0a18e28a322 232 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 233 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 234 return;
WD40andTape 6:f0a18e28a322 235 }
WD40andTape 6:f0a18e28a322 236 //printf("Message received.\r\n");
WD40andTape 7:5b6a2cefbf3b 237 input = hlcomms.consume_message( recv_buffer );
WD40andTape 1:2a43cf183a62 238
WD40andTape 7:5b6a2cefbf3b 239 // PROCESS INPUT
WD40andTape 7:5b6a2cefbf3b 240 double dblTargetChambLen_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
WD40andTape 6:f0a18e28a322 241 printf("REPLAN, %f\r\n",input.duration);
WD40andTape 7:5b6a2cefbf3b 242 // Update front segment
WD40andTape 6:f0a18e28a322 243 dblTargetChambLen_mm[0] = input.psi[0][0]*1000;
WD40andTape 6:f0a18e28a322 244 dblTargetChambLen_mm[1] = input.psi[0][1]*1000;
WD40andTape 6:f0a18e28a322 245 dblTargetChambLen_mm[2] = input.psi[0][2]*1000;
WD40andTape 7:5b6a2cefbf3b 246 // Update mid segment
WD40andTape 6:f0a18e28a322 247 dblTargetChambLen_mm[6] = input.psi[1][0]*1000;
WD40andTape 7:5b6a2cefbf3b 248 dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; // Same because two pumps are used
WD40andTape 7:5b6a2cefbf3b 249 // Update rear segment
WD40andTape 6:f0a18e28a322 250 dblTargetChambLen_mm[3] = input.psi[2][0]*1000;
WD40andTape 6:f0a18e28a322 251 dblTargetChambLen_mm[4] = input.psi[2][1]*1000;
WD40andTape 6:f0a18e28a322 252 dblTargetChambLen_mm[5] = input.psi[2][2]*1000;
dofydoink 0:607bc887b6e0 253
WD40andTape 6:f0a18e28a322 254 bool isTimeChanged = 0;
WD40andTape 6:f0a18e28a322 255 double dblMaxRecalculatedTime = input.duration;
WD40andTape 7:5b6a2cefbf3b 256 mutPathIn.lock(); // Lock variables to avoid race condition
WD40andTape 4:303584310071 257 for (ii = 0; ii< N_CHANNELS; ii++) {
dofydoink 0:607bc887b6e0 258 //check to see if positions are achievable
WD40andTape 6:f0a18e28a322 259 /*if(dblTargetChambLen_mm[ii]>DBL_MAX_CHAMBER_LENGTHS_MM[ii]) {
WD40andTape 4:303584310071 260 dblTargetChambLen_mm[ii] = DBL_MAX_CHAMBER_LENGTHS_MM[ii];
WD40andTape 6:f0a18e28a322 261 isTimeChanged = 1;
dofydoink 0:607bc887b6e0 262 }
WD40andTape 4:303584310071 263 if(dblTargetChambLen_mm[ii]<0.0) {
WD40andTape 6:f0a18e28a322 264 dblTargetChambLen_mm[ii] = 0.0;
WD40andTape 6:f0a18e28a322 265 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 266 }*/
WD40andTape 6:f0a18e28a322 267 dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
WD40andTape 6:f0a18e28a322 268 //!! LIMIT CHAMBER LENGTHS TOO
WD40andTape 6:f0a18e28a322 269 if( dblTargetActPos_mm[ii]<0.0 || dblTargetActPos_mm[ii]>25.0 ) {
WD40andTape 6:f0a18e28a322 270 dblTargetActPos_mm[ii] = min( max( 0.0 , dblTargetActPos_mm[ii] ) , 25.0 );
WD40andTape 6:f0a18e28a322 271 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 272 }
WD40andTape 6:f0a18e28a322 273 }
WD40andTape 6:f0a18e28a322 274 double dblActPosChange;
WD40andTape 6:f0a18e28a322 275 short sgn;
WD40andTape 7:5b6a2cefbf3b 276 for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
WD40andTape 6:f0a18e28a322 277 dblActPosChange = dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii];
WD40andTape 6:f0a18e28a322 278 if( fabs(dblActPosChange) < DBL_PATH_TOLERANCE ) {
WD40andTape 6:f0a18e28a322 279 dblActPosChange = 0.0;
WD40andTape 6:f0a18e28a322 280 isTimeChanged = 1;
dofydoink 0:607bc887b6e0 281 }
WD40andTape 6:f0a18e28a322 282 if( input.duration < 0.000000001 ) {
WD40andTape 6:f0a18e28a322 283 sgn = (dblActPosChange > 0) ? 1 : ((dblActPosChange < 0) ? -1 : 0);
WD40andTape 6:f0a18e28a322 284 dblVelocity_mmps[ii] = sgn * MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 285 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 286 } else {
WD40andTape 6:f0a18e28a322 287 dblVelocity_mmps[ii] = dblActPosChange / input.duration;
WD40andTape 6:f0a18e28a322 288 }
WD40andTape 7:5b6a2cefbf3b 289 // Check to see if velocities are achievable
WD40andTape 6:f0a18e28a322 290 if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
WD40andTape 6:f0a18e28a322 291 if(dblVelocity_mmps[ii]>0) {
WD40andTape 6:f0a18e28a322 292 dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 293 } else {
WD40andTape 6:f0a18e28a322 294 dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 295 }
WD40andTape 6:f0a18e28a322 296 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 297 }
WD40andTape 6:f0a18e28a322 298 double dblRecalculatedTime;
WD40andTape 6:f0a18e28a322 299 if( fabs(dblVelocity_mmps[ii]) < 0.000000001 ) {
WD40andTape 6:f0a18e28a322 300 dblRecalculatedTime = 0;
WD40andTape 6:f0a18e28a322 301 } else {
WD40andTape 6:f0a18e28a322 302 dblRecalculatedTime = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblVelocity_mmps[ii];
WD40andTape 6:f0a18e28a322 303 }
WD40andTape 6:f0a18e28a322 304 if( dblRecalculatedTime > dblMaxRecalculatedTime ) {
WD40andTape 6:f0a18e28a322 305 dblMaxRecalculatedTime = dblRecalculatedTime;
WD40andTape 6:f0a18e28a322 306 }
WD40andTape 6:f0a18e28a322 307 }
WD40andTape 6:f0a18e28a322 308 if( isTimeChanged ) {
WD40andTape 6:f0a18e28a322 309 if( dblMaxRecalculatedTime < input.duration ) {
WD40andTape 6:f0a18e28a322 310 dblMaxRecalculatedTime = input.duration;
WD40andTape 6:f0a18e28a322 311 }
WD40andTape 7:5b6a2cefbf3b 312 for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
WD40andTape 6:f0a18e28a322 313 dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblMaxRecalculatedTime;
WD40andTape 6:f0a18e28a322 314 }
WD40andTape 6:f0a18e28a322 315 }
WD40andTape 7:5b6a2cefbf3b 316 mutPathIn.unlock(); // Unlock mutex
WD40andTape 6:f0a18e28a322 317
WD40andTape 6:f0a18e28a322 318 printf("Sending message...\r\n");
WD40andTape 6:f0a18e28a322 319 // SEND MESSAGE
WD40andTape 6:f0a18e28a322 320 memset(send_buffer, 0, sizeof(send_buffer));
WD40andTape 6:f0a18e28a322 321 _snprintf(send_buffer,64,"%f",dblMaxRecalculatedTime);
WD40andTape 6:f0a18e28a322 322 message_to_send = send_buffer;
WD40andTape 7:5b6a2cefbf3b 323 error_code = hlcomms.interfaces.clt_sock.send(message_to_send, strlen(message_to_send));
WD40andTape 6:f0a18e28a322 324 if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 325 printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 326 "Perhaps the server socket is not bound or not set to listen for connections? "
WD40andTape 6:f0a18e28a322 327 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 328 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 329 return;
WD40andTape 6:f0a18e28a322 330 }
WD40andTape 6:f0a18e28a322 331 printf("Message sent.\r\n");
WD40andTape 6:f0a18e28a322 332 }
WD40andTape 6:f0a18e28a322 333
dofydoink 0:607bc887b6e0 334 }
dofydoink 0:607bc887b6e0 335
WD40andTape 4:303584310071 336 void CalculateSmoothPath() {
dofydoink 5:712e7634c779 337 int jj;
WD40andTape 1:2a43cf183a62 338 double dblMeasuredSampleTime;
WD40andTape 7:5b6a2cefbf3b 339 double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; // The current position of the smooth path (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 340 //double dblPosition_mtrs[N_CHANNELS]; // The actual chamber lengths in meters given as the change in length relative to neutral (should always be >=0)
WD40andTape 7:5b6a2cefbf3b 341 //double dblPressure_bar[N_CHANNELS]; // The pressure in a given chamber in bar (1 bar = 100,000 Pa)
WD40andTape 4:303584310071 342 while(1) {
dofydoink 0:607bc887b6e0 343 semPathPlan.wait();
WD40andTape 1:2a43cf183a62 344
WD40andTape 1:2a43cf183a62 345 // If run time is more than 50 us from expected, calculate from measured time step
WD40andTape 4:303584310071 346 if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50) {
dofydoink 5:712e7634c779 347 dblMeasuredSampleTime = timer.read();
WD40andTape 4:303584310071 348 } else {
WD40andTape 1:2a43cf183a62 349 dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
WD40andTape 1:2a43cf183a62 350 }
WD40andTape 4:303584310071 351 timer.reset();
WD40andTape 1:2a43cf183a62 352
dofydoink 5:712e7634c779 353 for(jj = 0; jj < N_CHANNELS; jj++) {
WD40andTape 7:5b6a2cefbf3b 354 //dblPressure_bar[jj] = ReadADCPressure_bar(jj); // Read pressure from channel
WD40andTape 7:5b6a2cefbf3b 355 //dblPosition_mtrs[jj] = ReadADCPosition_mtrs(jj); // Read position from channel
WD40andTape 1:2a43cf183a62 356
WD40andTape 7:5b6a2cefbf3b 357 // Calculate next step in linear path
WD40andTape 7:5b6a2cefbf3b 358 mutPathIn.lock(); // Lock relevant mutex
WD40andTape 7:5b6a2cefbf3b 359 dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime;
WD40andTape 7:5b6a2cefbf3b 360 if(dblLinearPathCurrentPos_mm[jj] < 0.0) {
WD40andTape 7:5b6a2cefbf3b 361 dblLinearPathCurrentPos_mm[jj] = 0.00;
WD40andTape 7:5b6a2cefbf3b 362 }
WD40andTape 7:5b6a2cefbf3b 363 // Check tolerance
WD40andTape 7:5b6a2cefbf3b 364 if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= DBL_PATH_TOLERANCE) {
WD40andTape 7:5b6a2cefbf3b 365 dblVelocity_mmps[jj] = 0.0; // Stop linear path generation when linear path is within tolerance of target position
WD40andTape 7:5b6a2cefbf3b 366 }
WD40andTape 7:5b6a2cefbf3b 367 mutPathIn.unlock(); // Unlock relevant mutex
dofydoink 0:607bc887b6e0 368
WD40andTape 7:5b6a2cefbf3b 369 // Calculate next step in smooth path
dofydoink 5:712e7634c779 370 dblSmoothPathCurrentPos_mm[jj] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
WD40andTape 7:5b6a2cefbf3b 371 mutChannel[jj].lock(); // MUTEX LOCK
WD40andTape 7:5b6a2cefbf3b 372 intDemandPos_Tx[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);// Convert to a 13-bit number
WD40andTape 7:5b6a2cefbf3b 373 intDemandPos_Tx[jj] = intDemandPos_Tx[jj] & 0x1FFF; // Ensure number is 13-bit
WD40andTape 7:5b6a2cefbf3b 374 mutChannel[jj].unlock(); // MUTEX UNLOCK
dofydoink 0:607bc887b6e0 375
dofydoink 5:712e7634c779 376 isDataReady[jj] = 1;//signal that data ready
WD40andTape 4:303584310071 377 } // end for
WD40andTape 7:5b6a2cefbf3b 378
WD40andTape 6:f0a18e28a322 379 //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
WD40andTape 6:f0a18e28a322 380 /*printf("%f, %f, %f, %f, %f, %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblLinearPathCurrentPos_mm[1],dblLinearPathCurrentPos_mm[2],
WD40andTape 6:f0a18e28a322 381 dblLinearPathCurrentPos_mm[3],dblLinearPathCurrentPos_mm[4],dblLinearPathCurrentPos_mm[5],dblLinearPathCurrentPos_mm[6],dblLinearPathCurrentPos_mm[7]);*/
WD40andTape 6:f0a18e28a322 382 //printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
WD40andTape 7:5b6a2cefbf3b 383
WD40andTape 4:303584310071 384 } // end while
WD40andTape 3:c83291bf9fd2 385 }
dofydoink 0:607bc887b6e0 386
WD40andTape 7:5b6a2cefbf3b 387 int main() {
WD40andTape 4:303584310071 388 int ii;
WD40andTape 7:5b6a2cefbf3b 389 // Initialise relevant variables
WD40andTape 7:5b6a2cefbf3b 390 for(ii = 0; ii<N_CHANNELS; ii++) {
WD40andTape 7:5b6a2cefbf3b 391 // All chip selects in off state
dofydoink 0:607bc887b6e0 392 cs_LL[ii] = 1;
dofydoink 0:607bc887b6e0 393 cs_ADC[ii] = 1;
WD40andTape 7:5b6a2cefbf3b 394 // Data ready flags set to not ready
WD40andTape 1:2a43cf183a62 395 isDataReady[ii] = 0;
WD40andTape 7:5b6a2cefbf3b 396 }
dofydoink 0:607bc887b6e0 397
WD40andTape 7:5b6a2cefbf3b 398 pinReset = 1; // Initialise reset pin to not reset the controllers.
dofydoink 5:712e7634c779 399 wait(0.25);
WD40andTape 7:5b6a2cefbf3b 400 pinReset=0; // Reset controllers to be safe
dofydoink 5:712e7634c779 401 wait(0.25);
WD40andTape 7:5b6a2cefbf3b 402 pinReset = 1; // Ready to go
dofydoink 0:607bc887b6e0 403
WD40andTape 7:5b6a2cefbf3b 404 pc.baud(BAUD_RATE);
dofydoink 0:607bc887b6e0 405 printf("Hi, there! I'll be your mid-level controller for today.\r\n");
dofydoink 5:712e7634c779 406 wait(3);
WD40andTape 7:5b6a2cefbf3b 407 t.start(callback(&queue, &EventQueue::dispatch_forever)); // Start the event queue
dofydoink 0:607bc887b6e0 408
WD40andTape 7:5b6a2cefbf3b 409 // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 3:c83291bf9fd2 410 pinGate0.rise(&rise0);
WD40andTape 3:c83291bf9fd2 411 pinGate1.rise(&rise1);
WD40andTape 3:c83291bf9fd2 412 pinGate2.rise(&rise2);
WD40andTape 3:c83291bf9fd2 413 pinGate3.rise(&rise3);
WD40andTape 3:c83291bf9fd2 414 pinGate4.rise(&rise4);
WD40andTape 3:c83291bf9fd2 415 pinGate5.rise(&rise5);
WD40andTape 3:c83291bf9fd2 416 pinGate6.rise(&rise6);
WD40andTape 3:c83291bf9fd2 417 pinGate7.rise(&rise7);
WD40andTape 7:5b6a2cefbf3b 418 // Set up fall interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 3:c83291bf9fd2 419 pinGate0.fall(&fall0);
WD40andTape 3:c83291bf9fd2 420 pinGate1.fall(&fall1);
WD40andTape 3:c83291bf9fd2 421 pinGate2.fall(&fall2);
WD40andTape 3:c83291bf9fd2 422 pinGate3.fall(&fall3);
WD40andTape 3:c83291bf9fd2 423 pinGate4.fall(&fall4);
WD40andTape 3:c83291bf9fd2 424 pinGate5.fall(&fall5);
WD40andTape 3:c83291bf9fd2 425 pinGate6.fall(&fall6);
WD40andTape 3:c83291bf9fd2 426 pinGate7.fall(&fall7);
dofydoink 5:712e7634c779 427
dofydoink 0:607bc887b6e0 428 timer.start();
WD40andTape 7:5b6a2cefbf3b 429
WD40andTape 7:5b6a2cefbf3b 430 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
WD40andTape 7:5b6a2cefbf3b 431 threadSmoothPathPlan.start(CalculateSmoothPath); // Start planning thread
WD40andTape 7:5b6a2cefbf3b 432
WD40andTape 7:5b6a2cefbf3b 433 PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); // Set up planning thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 434
WD40andTape 7:5b6a2cefbf3b 435 Thread::wait(1);
WD40andTape 1:2a43cf183a62 436 while(1) {
WD40andTape 1:2a43cf183a62 437 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 438 }
WD40andTape 7:5b6a2cefbf3b 439 }