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@6:f0a18e28a322, 2018-08-03 (annotated)

Committer:
WD40andTape
Date:
Fri Aug 03 11:08:12 2018 +0000
Revision:
6:f0a18e28a322
Parent:
5:712e7634c779
Child:
7:5b6a2cefbf3b
Successfully tested full HL/ML/SL setup.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dofydoink 0:607bc887b6e0 1 #include "mbed.h"
dofydoink 0:607bc887b6e0 2 #include "math.h"
WD40andTape 6:f0a18e28a322 3 #include "mbed_events.h"
dofydoink 0:607bc887b6e0 4
WD40andTape 6:f0a18e28a322 5 // COMMS IMPORTS
WD40andTape 6:f0a18e28a322 6 #include <sstream> // stringstream
WD40andTape 6:f0a18e28a322 7 #include <vector> // vector
WD40andTape 6:f0a18e28a322 8 #include <string.h> // strtok
dofydoink 0:607bc887b6e0 9 //#include "mbed.h"
WD40andTape 6:f0a18e28a322 10 #include "EthernetInterface.h"
WD40andTape 6:f0a18e28a322 11 #include "TCPServer.h"
WD40andTape 6:f0a18e28a322 12 #include "TCPSocket.h"
WD40andTape 6:f0a18e28a322 13
WD40andTape 6:f0a18e28a322 14 // COMMS SETTINGS
WD40andTape 6:f0a18e28a322 15 const bool IS_DHCP = false;
WD40andTape 6:f0a18e28a322 16 #define PORT 80
WD40andTape 6:f0a18e28a322 17
WD40andTape 6:f0a18e28a322 18 // COMMS STRUCT DECLARATIONS
WD40andTape 6:f0a18e28a322 19 struct msg_format {
WD40andTape 6:f0a18e28a322 20 double psi[3][3];
WD40andTape 6:f0a18e28a322 21 double duration;
WD40andTape 6:f0a18e28a322 22 };
WD40andTape 6:f0a18e28a322 23 struct comms_interfaces {
WD40andTape 6:f0a18e28a322 24 EthernetInterface eth; // Indicates which NetworkInterface the socket should be created on
WD40andTape 6:f0a18e28a322 25 TCPServer srv; // Provides the ability to accept incoming TCP connections
WD40andTape 6:f0a18e28a322 26 TCPSocket clt_sock; // Provides the ability to send a stream of data over TCP
WD40andTape 6:f0a18e28a322 27 SocketAddress clt_addr; // Represents the IP address and port pair of a unique network endpoint
WD40andTape 6:f0a18e28a322 28 };
WD40andTape 6:f0a18e28a322 29
WD40andTape 6:f0a18e28a322 30 // COMMS FUNCTION DECLARATIONS
WD40andTape 6:f0a18e28a322 31 int setup_server(void);
WD40andTape 6:f0a18e28a322 32 int accept_connection(void);
WD40andTape 6:f0a18e28a322 33 void close_server(void);
WD40andTape 6:f0a18e28a322 34 msg_format consume_message( unsigned char * msg );
WD40andTape 6:f0a18e28a322 35
WD40andTape 6:f0a18e28a322 36 // COMMS GLOBAL VARIABLES
WD40andTape 6:f0a18e28a322 37 struct comms_interfaces comms;
dofydoink 0:607bc887b6e0 38
dofydoink 0:607bc887b6e0 39 //ADC SPI stuff
dofydoink 0:607bc887b6e0 40 #define PREAMBLE 0x06
dofydoink 0:607bc887b6e0 41 #define CHAN_1 0x30
dofydoink 0:607bc887b6e0 42 #define CHAN_2 0x70
dofydoink 0:607bc887b6e0 43 #define CHAN_3 0xB0
dofydoink 0:607bc887b6e0 44 #define CHAN_4 0xF0
dofydoink 0:607bc887b6e0 45
dofydoink 0:607bc887b6e0 46 #define DATA_MASK 0x0F
dofydoink 0:607bc887b6e0 47
WD40andTape 1:2a43cf183a62 48 //simple channel selection
dofydoink 0:607bc887b6e0 49 #define ADC_PRESSURE 1
dofydoink 0:607bc887b6e0 50 #define ADC_POSITION 3
WD40andTape 1:2a43cf183a62 51 //---------------------
dofydoink 0:607bc887b6e0 52
dofydoink 0:607bc887b6e0 53 #define N_CHANNELS 8 //number of channels to control
dofydoink 0:607bc887b6e0 54 //1-3: front segment
dofydoink 0:607bc887b6e0 55 //4-6: rear segment
dofydoink 0:607bc887b6e0 56 //7-8: mid segment
dofydoink 0:607bc887b6e0 57
dofydoink 0:607bc887b6e0 58 //parameters to manually change
dofydoink 0:607bc887b6e0 59 #define LOW_LEVEL_SPI_FREQUENCY 10000000
dofydoink 0:607bc887b6e0 60
WD40andTape 6:f0a18e28a322 61 #define PATH_SAMPLE_TIME_S 0.005 //0.109
dofydoink 0:607bc887b6e0 62
dofydoink 0:607bc887b6e0 63 #define MAX_LENGTH_MM 100.0
WD40andTape 3:c83291bf9fd2 64 #define MAX_ACTUATOR_LENGTH 52.2
dofydoink 0:607bc887b6e0 65 #define MAX_SPEED_MMPS 24.3457
WD40andTape 4:303584310071 66 #define PATH_TOLERANCE_MM 0.2 //how close the linear path must get to the target position before considering it a success.
dofydoink 0:607bc887b6e0 67
WD40andTape 6:f0a18e28a322 68 //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 6:f0a18e28a322 69 //const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {0.30586,0.30586,0.30586,0.30586,0.30586,0.4588,0.4588};// covert from chamber lengths to actuator lengths
WD40andTape 6:f0a18e28a322 70 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 71 const double DBL_SMOOTHING_FACTOR = 0.5; // 0<x<=1, where 1 is no smoothing
WD40andTape 6:f0a18e28a322 72 const double DBL_PATH_TOLERANCE = MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance in mm with 5% tolerance
WD40andTape 4:303584310071 73 //-----These are the variables being shared between High-Level and Mid-Level-----------------------------------------------//
dofydoink 0:607bc887b6e0 74
dofydoink 0:607bc887b6e0 75 //
dofydoink 0:607bc887b6e0 76 Semaphore semReplan(1);// this must be set every time new high-level transmissions are received to allow replanning to take place!
dofydoink 0:607bc887b6e0 77 //--------------------------------------------------------------------------------------------------------------------------//
dofydoink 0:607bc887b6e0 78
dofydoink 0:607bc887b6e0 79 //boolean flag to indicate that new target information has been received
dofydoink 0:607bc887b6e0 80 //bool blnReplan;//set this when new transmission is received over ethernet
dofydoink 0:607bc887b6e0 81
dofydoink 0:607bc887b6e0 82 //path variables
WD40andTape 4:303584310071 83 double dblVelocity_mmps[N_CHANNELS] = { 0.0 };//the linear path velocity (not sent to actuator) KEEP GLOBAL
WD40andTape 4:303584310071 84 double dblLinearPathCurrentPos_mm[N_CHANNELS]={ 0.0 }; //the current position of the linear path (not sent to actuator) //KEEP GLOBAL
WD40andTape 4:303584310071 85 double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; //The final target position for the actuator KEEP GLOBAL
dofydoink 0:607bc887b6e0 86
WD40andTape 4:303584310071 87 int intDemandPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator KEEP GLOBAL
dofydoink 0:607bc887b6e0 88
WD40andTape 4:303584310071 89 char chrErrorFlag[N_CHANNELS];// 3 error bits from LL KEEP GLOBAL
dofydoink 0:607bc887b6e0 90
WD40andTape 4:303584310071 91 //pin declarations----------------
dofydoink 0:607bc887b6e0 92 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
dofydoink 0:607bc887b6e0 93
WD40andTape 3:c83291bf9fd2 94 InterruptIn pinGate6(PE_11); //this pin HAS TO BE defined before SPI set up. No Clue Why.
WD40andTape 3:c83291bf9fd2 95
WD40andTape 3:c83291bf9fd2 96 SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
WD40andTape 3:c83291bf9fd2 97
WD40andTape 3:c83291bf9fd2 98 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 99 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 100
WD40andTape 3:c83291bf9fd2 101 DigitalOut pinCheck(PE_5);
WD40andTape 3:c83291bf9fd2 102
WD40andTape 3:c83291bf9fd2 103 //these interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
WD40andTape 3:c83291bf9fd2 104 InterruptIn pinGate0(PF_11);
WD40andTape 3:c83291bf9fd2 105 InterruptIn pinGate1(PG_14);
WD40andTape 3:c83291bf9fd2 106 InterruptIn pinGate2(PF_15);
WD40andTape 3:c83291bf9fd2 107 InterruptIn pinGate3(PF_12);
WD40andTape 3:c83291bf9fd2 108 InterruptIn pinGate4(PF_3);
WD40andTape 3:c83291bf9fd2 109 InterruptIn pinGate5(PF_13);
WD40andTape 4:303584310071 110 //InterruptIn pinGate6(PE_11); //see above nonsense
WD40andTape 3:c83291bf9fd2 111 InterruptIn pinGate7(PE_13);
dofydoink 0:607bc887b6e0 112
dofydoink 0:607bc887b6e0 113 DigitalOut pinReset(PD_2); //reset pin for all controllers.
dofydoink 0:607bc887b6e0 114
WD40andTape 3:c83291bf9fd2 115
dofydoink 0:607bc887b6e0 116 EventQueue queue(32 * EVENTS_EVENT_SIZE);
dofydoink 0:607bc887b6e0 117
dofydoink 0:607bc887b6e0 118 Thread t(osPriorityRealtime);
dofydoink 0:607bc887b6e0 119
WD40andTape 4:303584310071 120 Thread threadReceiveAndReplan(osPriorityBelowNormal);
WD40andTape 4:303584310071 121 Thread threadSmoothPathPlan(osPriorityNormal);
dofydoink 0:607bc887b6e0 122
dofydoink 0:607bc887b6e0 123 Thread threadSimulateDemand(osPriorityHigh);
dofydoink 0:607bc887b6e0 124
WD40andTape 1:2a43cf183a62 125 Mutex mutChannel[N_CHANNELS];
WD40andTape 4:303584310071 126 Mutex mutPathIn;
dofydoink 0:607bc887b6e0 127
dofydoink 0:607bc887b6e0 128 Semaphore semPathPlan(1);//
dofydoink 0:607bc887b6e0 129
dofydoink 0:607bc887b6e0 130 Timer timer;//timers are nice
dofydoink 0:607bc887b6e0 131
dofydoink 0:607bc887b6e0 132 int ThreadID[N_CHANNELS];
dofydoink 0:607bc887b6e0 133
WD40andTape 1:2a43cf183a62 134 bool isDataReady[N_CHANNELS]; // flag to indicate path data is ready for transmission to low level.
dofydoink 0:607bc887b6e0 135
dofydoink 5:712e7634c779 136 double dblGlobalTest;
dofydoink 5:712e7634c779 137 int intGlobalTest;
WD40andTape 4:303584310071 138 double ReadADCPosition_mtrs(int channel)
dofydoink 0:607bc887b6e0 139 {
dofydoink 0:607bc887b6e0 140 unsigned int outputA;
dofydoink 0:607bc887b6e0 141 unsigned int outputB;
WD40andTape 3:c83291bf9fd2 142 int output;
WD40andTape 3:c83291bf9fd2 143 double dblOutput;
dofydoink 0:607bc887b6e0 144
dofydoink 0:607bc887b6e0 145 spi.format(8,0);
dofydoink 0:607bc887b6e0 146 spi.frequency(1000000);
dofydoink 0:607bc887b6e0 147
dofydoink 0:607bc887b6e0 148 cs_ADC[channel] = 0;
dofydoink 0:607bc887b6e0 149 spi.write(PREAMBLE);
dofydoink 0:607bc887b6e0 150 outputA = spi.write(CHAN_3);
dofydoink 0:607bc887b6e0 151 outputB = spi.write(0xFF);
WD40andTape 1:2a43cf183a62 152 cs_ADC[channel]= 1;
dofydoink 0:607bc887b6e0 153
dofydoink 0:607bc887b6e0 154 outputA = outputA & DATA_MASK;
dofydoink 0:607bc887b6e0 155 outputA = outputA<<8;
dofydoink 0:607bc887b6e0 156 output = (outputA | outputB);
WD40andTape 3:c83291bf9fd2 157 output = 4095- output;
dofydoink 5:712e7634c779 158 dblOutput = (double) (output);
dofydoink 5:712e7634c779 159 dblOutput = dblOutput*0.0229 - 21.582;
dofydoink 5:712e7634c779 160 //dblOutput = dblOutput - 21.78;
WD40andTape 3:c83291bf9fd2 161 return dblOutput;
dofydoink 5:712e7634c779 162 //dblOutput = (double) ((output/4095)*100.0);
dofydoink 5:712e7634c779 163 // dblOutput = dblOutput - 36.727717;
dofydoink 5:712e7634c779 164 // return dblOutput;
dofydoink 0:607bc887b6e0 165 }
dofydoink 0:607bc887b6e0 166
WD40andTape 4:303584310071 167 double ReadADCPressure_bar(int channel)
dofydoink 0:607bc887b6e0 168 {
dofydoink 0:607bc887b6e0 169 unsigned int outputA;
dofydoink 0:607bc887b6e0 170 unsigned int outputB;
WD40andTape 3:c83291bf9fd2 171 int output;
WD40andTape 3:c83291bf9fd2 172 double dblOutput;
dofydoink 0:607bc887b6e0 173
dofydoink 0:607bc887b6e0 174 spi.format(8,0);
dofydoink 0:607bc887b6e0 175 spi.frequency(1000000);
dofydoink 0:607bc887b6e0 176
dofydoink 0:607bc887b6e0 177 cs_ADC[channel] = 0;
dofydoink 0:607bc887b6e0 178 spi.write(PREAMBLE);
dofydoink 0:607bc887b6e0 179 outputA = spi.write(CHAN_1);
dofydoink 0:607bc887b6e0 180 outputB = spi.write(0xFF);
dofydoink 0:607bc887b6e0 181 cs_ADC[channel] = 1;
dofydoink 0:607bc887b6e0 182
dofydoink 0:607bc887b6e0 183 outputA = outputA & DATA_MASK;
dofydoink 0:607bc887b6e0 184 outputA = outputA<<8;
dofydoink 0:607bc887b6e0 185 output = (outputA | outputB);
dofydoink 0:607bc887b6e0 186
dofydoink 5:712e7634c779 187 dblOutput = (double)(output);
dofydoink 5:712e7634c779 188 dblOutput = dblOutput-502.0;
dofydoink 5:712e7634c779 189 dblOutput = dblOutput/4095.0*8.0;
WD40andTape 3:c83291bf9fd2 190 return dblOutput;
dofydoink 0:607bc887b6e0 191 }
dofydoink 0:607bc887b6e0 192
WD40andTape 1:2a43cf183a62 193 void SendReceiveData(int channel)
dofydoink 0:607bc887b6e0 194 {
WD40andTape 3:c83291bf9fd2 195
WD40andTape 4:303584310071 196
WD40andTape 4:303584310071 197 int intPosSPI_Rx[N_CHANNELS]; //13 bit value received over SPI from the actuator
WD40andTape 4:303584310071 198
WD40andTape 4:303584310071 199
dofydoink 0:607bc887b6e0 200 //get data from controller
dofydoink 0:607bc887b6e0 201 spi.format(16,2);
dofydoink 0:607bc887b6e0 202 spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 4:303584310071 203 mutChannel[channel].lock();//lock mutex for specific Channel
dofydoink 0:607bc887b6e0 204 cs_LL[channel] = 0;//select relevant chip
WD40andTape 1:2a43cf183a62 205 intPosSPI_Rx[channel] = spi.write(intDemandPos_Tx[channel]); //transmit & receive
dofydoink 0:607bc887b6e0 206 cs_LL[channel] = 1;//deselect chip
WD40andTape 1:2a43cf183a62 207 isDataReady[channel] = 0;//data no longer ready, i.e. we now require new data
dofydoink 5:712e7634c779 208 if(channel == 0) {
dofydoink 5:712e7634c779 209 intGlobalTest = intPosSPI_Rx[channel];
dofydoink 5:712e7634c779 210 dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
dofydoink 5:712e7634c779 211 }
WD40andTape 1:2a43cf183a62 212
dofydoink 0:607bc887b6e0 213
dofydoink 0:607bc887b6e0 214 //sort out received data
dofydoink 0:607bc887b6e0 215 chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
WD40andTape 4:303584310071 216
dofydoink 0:607bc887b6e0 217 intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
WD40andTape 4:303584310071 218 //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
WD40andTape 4:303584310071 219 mutChannel[channel].unlock();//unlock mutex for specific channel
WD40andTape 3:c83291bf9fd2 220 //printf("%d, %d\r\n",intPosSPI_Rx[0],intDemandPos_Tx[0]);
WD40andTape 3:c83291bf9fd2 221
WD40andTape 3:c83291bf9fd2 222 }
WD40andTape 3:c83291bf9fd2 223
WD40andTape 4:303584310071 224
dofydoink 0:607bc887b6e0 225
dofydoink 0:607bc887b6e0 226 //common rise handler function
dofydoink 0:607bc887b6e0 227
dofydoink 0:607bc887b6e0 228 void common_rise_handler(int channel)
dofydoink 0:607bc887b6e0 229 {
WD40andTape 3:c83291bf9fd2 230 pinCheck = 1;
dofydoink 0:607bc887b6e0 231 //check if data is ready for tranmission
WD40andTape 1:2a43cf183a62 232 if (isDataReady[channel])
dofydoink 0:607bc887b6e0 233 {
dofydoink 0:607bc887b6e0 234 // Add transmit task to event queue
WD40andTape 1:2a43cf183a62 235 ThreadID[channel] = queue.call(SendReceiveData,channel);//schedule transmission
dofydoink 0:607bc887b6e0 236 }
dofydoink 0:607bc887b6e0 237 }
dofydoink 0:607bc887b6e0 238
WD40andTape 3:c83291bf9fd2 239
dofydoink 0:607bc887b6e0 240
dofydoink 0:607bc887b6e0 241
dofydoink 0:607bc887b6e0 242 //common_fall handler functions
dofydoink 0:607bc887b6e0 243 void common_fall_handler(int channel)
dofydoink 0:607bc887b6e0 244 {
WD40andTape 3:c83291bf9fd2 245 pinCheck = 0;
dofydoink 0:607bc887b6e0 246 //cancel relevant queued event
dofydoink 0:607bc887b6e0 247 queue.cancel(ThreadID[channel]);
dofydoink 0:607bc887b6e0 248 }
dofydoink 0:607bc887b6e0 249
dofydoink 0:607bc887b6e0 250 //stub rise functions
dofydoink 0:607bc887b6e0 251 void rise0(void) { common_rise_handler(0); }
dofydoink 0:607bc887b6e0 252 void rise1(void) { common_rise_handler(1); }
dofydoink 0:607bc887b6e0 253 void rise2(void) { common_rise_handler(2); }
dofydoink 0:607bc887b6e0 254 void rise3(void) { common_rise_handler(3); }
dofydoink 0:607bc887b6e0 255 void rise4(void) { common_rise_handler(4); }
dofydoink 0:607bc887b6e0 256 void rise5(void) { common_rise_handler(5); }
dofydoink 0:607bc887b6e0 257 void rise6(void) { common_rise_handler(6); }
dofydoink 0:607bc887b6e0 258 void rise7(void) { common_rise_handler(7); }
dofydoink 0:607bc887b6e0 259
dofydoink 0:607bc887b6e0 260 //stub fall functions
dofydoink 0:607bc887b6e0 261 void fall0(void) { common_fall_handler(0); }
dofydoink 0:607bc887b6e0 262 void fall1(void) { common_fall_handler(1); }
dofydoink 0:607bc887b6e0 263 void fall2(void) { common_fall_handler(2); }
dofydoink 0:607bc887b6e0 264 void fall3(void) { common_fall_handler(3); }
dofydoink 0:607bc887b6e0 265 void fall4(void) { common_fall_handler(4); }
dofydoink 0:607bc887b6e0 266 void fall5(void) { common_fall_handler(5); }
dofydoink 0:607bc887b6e0 267 void fall6(void) { common_fall_handler(6); }
dofydoink 0:607bc887b6e0 268 void fall7(void) { common_fall_handler(7); }
dofydoink 0:607bc887b6e0 269
WD40andTape 1:2a43cf183a62 270 void startPathPlan() // Plan a new linear path after receiving new target data
dofydoink 0:607bc887b6e0 271 {
WD40andTape 4:303584310071 272 semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
dofydoink 0:607bc887b6e0 273 }
dofydoink 0:607bc887b6e0 274
dofydoink 5:712e7634c779 275 double somedumbthing;
dofydoink 5:712e7634c779 276
dofydoink 0:607bc887b6e0 277 //this function will be called when a new transmission is received from high level
WD40andTape 4:303584310071 278 void ReceiveAndReplan()
dofydoink 0:607bc887b6e0 279 {
WD40andTape 4:303584310071 280 int ii;
WD40andTape 4:303584310071 281
WD40andTape 6:f0a18e28a322 282 //double dblPSI[3][3]; //the message from high-level containing the chamber lengths in meters in following format:
WD40andTape 4:303584310071 283 /*
WD40andTape 4:303584310071 284 {L(front,1), L(front,2), L(front,3);
WD40andTape 4:303584310071 285 L(mid,1) , L(mid,2) , L(mid,3);
WD40andTape 4:303584310071 286 L(rear,1) , L(rear,2) , L(rear,3);}
WD40andTape 4:303584310071 287 */
WD40andTape 6:f0a18e28a322 288 //double dblTargetTime_s; //the time in seconds desired to reach the target (>0...!)
dofydoink 5:712e7634c779 289 //double dblPosition_mtrs[N_CHANNELS]; //the actual chamber lengths in meters given as the change in length relative to neutral (should always be >=0)
dofydoink 5:712e7634c779 290 // double dblPressure_bar[N_CHANNELS]; //the pressure in a given chamber in bar (1 bar = 100,000 Pa).
dofydoink 5:712e7634c779 291
WD40andTape 4:303584310071 292
WD40andTape 4:303584310071 293 //!!!!!!!!!!!!!!! If received messages faster than path replanning, will get increasingly behind time --> need to throw away some instructions
WD40andTape 2:eea12b149dba 294
WD40andTape 4:303584310071 295 //receive
WD40andTape 4:303584310071 296
WD40andTape 6:f0a18e28a322 297 // Error codes: https://github.com/ARMmbed/mbed-os/blob/master/features/netsocket/nsapi_types.h
WD40andTape 6:f0a18e28a322 298 // http://man7.org/linux/man-pages/man3/errno.3.html
WD40andTape 6:f0a18e28a322 299
WD40andTape 6:f0a18e28a322 300 int error_code;
WD40andTape 6:f0a18e28a322 301
WD40andTape 6:f0a18e28a322 302 error_code = setup_server();
WD40andTape 6:f0a18e28a322 303 if( error_code == -1 ) return;
WD40andTape 6:f0a18e28a322 304 error_code = accept_connection();
WD40andTape 6:f0a18e28a322 305 if( error_code == -1 ) return;
WD40andTape 6:f0a18e28a322 306
WD40andTape 6:f0a18e28a322 307 unsigned char recv_buffer[1024];
WD40andTape 6:f0a18e28a322 308 struct msg_format input;
WD40andTape 6:f0a18e28a322 309 char send_buffer[65];
WD40andTape 6:f0a18e28a322 310 char * message_to_send;
WD40andTape 4:303584310071 311
WD40andTape 6:f0a18e28a322 312 while( true ) {
WD40andTape 6:f0a18e28a322 313 // RECEIVE MESSAGE
WD40andTape 6:f0a18e28a322 314 memset(recv_buffer, 0, sizeof(recv_buffer));
WD40andTape 6:f0a18e28a322 315 error_code = comms.clt_sock.recv(recv_buffer, 1024); // Blocks until data is received
WD40andTape 6:f0a18e28a322 316 if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
WD40andTape 6:f0a18e28a322 317 printf("Client disconnected.\n\r");
WD40andTape 6:f0a18e28a322 318 close_server();
WD40andTape 6:f0a18e28a322 319 return;
WD40andTape 6:f0a18e28a322 320 } else if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 321 printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 322 "Perhaps the server socket is not connected to a remote host? "
WD40andTape 6:f0a18e28a322 323 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 6:f0a18e28a322 324 close_server();
WD40andTape 6:f0a18e28a322 325 return;
WD40andTape 6:f0a18e28a322 326 }
WD40andTape 6:f0a18e28a322 327 //printf("Message received.\r\n");
WD40andTape 6:f0a18e28a322 328 input = consume_message( recv_buffer );
WD40andTape 6:f0a18e28a322 329 //!! Set global variables to input with mutex
WD40andTape 1:2a43cf183a62 330
WD40andTape 6:f0a18e28a322 331 // Process input
WD40andTape 6:f0a18e28a322 332
WD40andTape 6:f0a18e28a322 333 double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
WD40andTape 6:f0a18e28a322 334 printf("REPLAN, %f\r\n",input.duration);
dofydoink 0:607bc887b6e0 335 //update front segment
WD40andTape 6:f0a18e28a322 336 dblTargetChambLen_mm[0] = input.psi[0][0]*1000;
WD40andTape 6:f0a18e28a322 337 dblTargetChambLen_mm[1] = input.psi[0][1]*1000;
WD40andTape 6:f0a18e28a322 338 dblTargetChambLen_mm[2] = input.psi[0][2]*1000;
dofydoink 0:607bc887b6e0 339 //update mid segment
WD40andTape 6:f0a18e28a322 340 dblTargetChambLen_mm[6] = input.psi[1][0]*1000;
dofydoink 0:607bc887b6e0 341 dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; //same because two pumps are used
dofydoink 0:607bc887b6e0 342 //update rear segment
WD40andTape 6:f0a18e28a322 343 dblTargetChambLen_mm[3] = input.psi[2][0]*1000;
WD40andTape 6:f0a18e28a322 344 dblTargetChambLen_mm[4] = input.psi[2][1]*1000;
WD40andTape 6:f0a18e28a322 345 dblTargetChambLen_mm[5] = input.psi[2][2]*1000;
dofydoink 0:607bc887b6e0 346
WD40andTape 6:f0a18e28a322 347 bool isTimeChanged = 0;
WD40andTape 6:f0a18e28a322 348 double dblMaxRecalculatedTime = input.duration;
WD40andTape 6:f0a18e28a322 349 mutPathIn.lock();//lock variables to avoid race condition
WD40andTape 4:303584310071 350 for (ii = 0; ii< N_CHANNELS; ii++) {
dofydoink 0:607bc887b6e0 351 //check to see if positions are achievable
WD40andTape 6:f0a18e28a322 352 /*if(dblTargetChambLen_mm[ii]>DBL_MAX_CHAMBER_LENGTHS_MM[ii]) {
WD40andTape 4:303584310071 353 dblTargetChambLen_mm[ii] = DBL_MAX_CHAMBER_LENGTHS_MM[ii];
WD40andTape 6:f0a18e28a322 354 isTimeChanged = 1;
dofydoink 0:607bc887b6e0 355 }
WD40andTape 4:303584310071 356 if(dblTargetChambLen_mm[ii]<0.0) {
WD40andTape 6:f0a18e28a322 357 dblTargetChambLen_mm[ii] = 0.0;
WD40andTape 6:f0a18e28a322 358 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 359 }*/
WD40andTape 6:f0a18e28a322 360 dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
WD40andTape 6:f0a18e28a322 361 //!! LIMIT CHAMBER LENGTHS TOO
WD40andTape 6:f0a18e28a322 362 if( dblTargetActPos_mm[ii]<0.0 || dblTargetActPos_mm[ii]>25.0 ) {
WD40andTape 6:f0a18e28a322 363 dblTargetActPos_mm[ii] = min( max( 0.0 , dblTargetActPos_mm[ii] ) , 25.0 );
WD40andTape 6:f0a18e28a322 364 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 365 }
WD40andTape 6:f0a18e28a322 366 }
WD40andTape 6:f0a18e28a322 367 double dblActPosChange;
WD40andTape 6:f0a18e28a322 368 short sgn;
WD40andTape 6:f0a18e28a322 369 for (ii = 0; ii< N_CHANNELS; ii++) {
WD40andTape 6:f0a18e28a322 370 //work out new velocities
WD40andTape 6:f0a18e28a322 371 dblActPosChange = dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii];
WD40andTape 6:f0a18e28a322 372 if( fabs(dblActPosChange) < DBL_PATH_TOLERANCE ) {
WD40andTape 6:f0a18e28a322 373 dblActPosChange = 0.0;
WD40andTape 6:f0a18e28a322 374 isTimeChanged = 1;
dofydoink 0:607bc887b6e0 375 }
WD40andTape 6:f0a18e28a322 376 if( input.duration < 0.000000001 ) {
WD40andTape 6:f0a18e28a322 377 sgn = (dblActPosChange > 0) ? 1 : ((dblActPosChange < 0) ? -1 : 0);
WD40andTape 6:f0a18e28a322 378 dblVelocity_mmps[ii] = sgn * MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 379 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 380 } else {
WD40andTape 6:f0a18e28a322 381 dblVelocity_mmps[ii] = dblActPosChange / input.duration;
WD40andTape 6:f0a18e28a322 382 }
WD40andTape 6:f0a18e28a322 383 //check to see if velocities are achievable
WD40andTape 6:f0a18e28a322 384 if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
WD40andTape 6:f0a18e28a322 385 if(dblVelocity_mmps[ii]>0) {
WD40andTape 6:f0a18e28a322 386 dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 387 } else {
WD40andTape 6:f0a18e28a322 388 dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 389 }
WD40andTape 6:f0a18e28a322 390 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 391 }
WD40andTape 6:f0a18e28a322 392 double dblRecalculatedTime;
WD40andTape 6:f0a18e28a322 393 if( fabs(dblVelocity_mmps[ii]) < 0.000000001 ) {
WD40andTape 6:f0a18e28a322 394 dblRecalculatedTime = 0;
WD40andTape 6:f0a18e28a322 395 } else {
WD40andTape 6:f0a18e28a322 396 dblRecalculatedTime = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblVelocity_mmps[ii];
WD40andTape 6:f0a18e28a322 397 }
WD40andTape 6:f0a18e28a322 398 if( dblRecalculatedTime > dblMaxRecalculatedTime ) {
WD40andTape 6:f0a18e28a322 399 dblMaxRecalculatedTime = dblRecalculatedTime;
WD40andTape 6:f0a18e28a322 400 }
WD40andTape 6:f0a18e28a322 401 }
WD40andTape 6:f0a18e28a322 402 if( isTimeChanged ) {
WD40andTape 6:f0a18e28a322 403 if( dblMaxRecalculatedTime < input.duration ) {
WD40andTape 6:f0a18e28a322 404 dblMaxRecalculatedTime = input.duration;
WD40andTape 6:f0a18e28a322 405 }
WD40andTape 6:f0a18e28a322 406 for (ii = 0; ii< N_CHANNELS; ii++) {
dofydoink 5:712e7634c779 407 //work out new velocities
WD40andTape 6:f0a18e28a322 408 dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblMaxRecalculatedTime;
WD40andTape 6:f0a18e28a322 409 }
WD40andTape 6:f0a18e28a322 410 }
dofydoink 0:607bc887b6e0 411
dofydoink 0:607bc887b6e0 412
WD40andTape 6:f0a18e28a322 413 //dblPressure_bar[ii] = 0.5;//read pressure from channel
WD40andTape 6:f0a18e28a322 414 //dblPosition_mtrs[ii] = 0.02; //read position from channel
WD40andTape 4:303584310071 415 //printf("%f\r\n", dblVelocity_mmps[0]);
WD40andTape 6:f0a18e28a322 416 mutPathIn.unlock(); //unlock mutex.
WD40andTape 6:f0a18e28a322 417
WD40andTape 6:f0a18e28a322 418 printf("Sending message...\r\n");
WD40andTape 6:f0a18e28a322 419 // SEND MESSAGE
WD40andTape 6:f0a18e28a322 420 memset(send_buffer, 0, sizeof(send_buffer));
WD40andTape 6:f0a18e28a322 421 _snprintf(send_buffer,64,"%f",dblMaxRecalculatedTime);
WD40andTape 6:f0a18e28a322 422 message_to_send = send_buffer;
WD40andTape 6:f0a18e28a322 423 error_code = comms.clt_sock.send(message_to_send, strlen(message_to_send));
WD40andTape 6:f0a18e28a322 424 if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 425 printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 426 "Perhaps the server socket is not bound or not set to listen for connections? "
WD40andTape 6:f0a18e28a322 427 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 6:f0a18e28a322 428 close_server();
WD40andTape 6:f0a18e28a322 429 return;
WD40andTape 6:f0a18e28a322 430 }
WD40andTape 6:f0a18e28a322 431 printf("Message sent.\r\n");
WD40andTape 6:f0a18e28a322 432 }
WD40andTape 6:f0a18e28a322 433
dofydoink 0:607bc887b6e0 434 }
dofydoink 0:607bc887b6e0 435
WD40andTape 2:eea12b149dba 436 // lock mutex
WD40andTape 2:eea12b149dba 437 // get variables
WD40andTape 2:eea12b149dba 438 // unlock mutex
WD40andTape 2:eea12b149dba 439 // do calculations
WD40andTape 2:eea12b149dba 440 // lock mutex
WD40andTape 2:eea12b149dba 441 // set variables
WD40andTape 4:303584310071 442 // unlock mutex
WD40andTape 2:eea12b149dba 443
WD40andTape 4:303584310071 444 void CalculateSmoothPath() {
dofydoink 5:712e7634c779 445 int jj;
WD40andTape 1:2a43cf183a62 446 double dblMeasuredSampleTime;
WD40andTape 4:303584310071 447 double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; //the current position of the smooth path (not sent to actuator)
dofydoink 5:712e7634c779 448 double dblPosition_mtrs[N_CHANNELS]; //the actual chamber lengths in meters given as the change in length relative to neutral (should always be >=0)
dofydoink 5:712e7634c779 449 double dblPressure_bar[N_CHANNELS]; //the pressure in a given chamber in bar (1 bar = 100,000 Pa).
WD40andTape 4:303584310071 450 while(1) {
dofydoink 0:607bc887b6e0 451 semPathPlan.wait();
WD40andTape 1:2a43cf183a62 452
WD40andTape 1:2a43cf183a62 453 // If run time is more than 50 us from expected, calculate from measured time step
WD40andTape 4:303584310071 454 if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50) {
dofydoink 5:712e7634c779 455 dblMeasuredSampleTime = timer.read();
WD40andTape 4:303584310071 456 } else {
WD40andTape 1:2a43cf183a62 457 dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
WD40andTape 1:2a43cf183a62 458 }
WD40andTape 4:303584310071 459 timer.reset();
WD40andTape 1:2a43cf183a62 460
dofydoink 5:712e7634c779 461 for(jj = 0; jj < N_CHANNELS; jj++) {
dofydoink 5:712e7634c779 462
dofydoink 5:712e7634c779 463 dblPressure_bar[jj] = ReadADCPressure_bar(jj);//read pressure from channel
dofydoink 5:712e7634c779 464 dblPosition_mtrs[jj] = ReadADCPosition_mtrs(jj); //read position from channel
dofydoink 5:712e7634c779 465 //DEBUGGING
dofydoink 5:712e7634c779 466 //if(jj == 0) printf("BEFORE: %f, %f, %f\r\n",dblVelocity_mmps[0],dblPosition_mtrs[0], dblMeasuredSampleTime);
WD40andTape 1:2a43cf183a62 467
dofydoink 5:712e7634c779 468 //calculate next step in linear path
dofydoink 5:712e7634c779 469 mutPathIn.lock();//lock relevant mutex
dofydoink 5:712e7634c779 470 dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime; // PATH_SAMPLE_TIME_S SHOULD BE MEASURED
dofydoink 5:712e7634c779 471 if(dblLinearPathCurrentPos_mm[jj] < 0.0) {
dofydoink 5:712e7634c779 472 dblLinearPathCurrentPos_mm[jj] = 0.00;
dofydoink 5:712e7634c779 473 }
WD40andTape 6:f0a18e28a322 474 /*if(dblLinearPathCurrentPos_mm[jj] > 0.0) {
WD40andTape 6:f0a18e28a322 475 dblLinearPathCurrentPos_mm[jj] = 0.00;
WD40andTape 6:f0a18e28a322 476 }*/
WD40andTape 6:f0a18e28a322 477
dofydoink 5:712e7634c779 478
dofydoink 5:712e7634c779 479 //check tolerance
dofydoink 5:712e7634c779 480 if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= DBL_PATH_TOLERANCE) {
dofydoink 5:712e7634c779 481 dblVelocity_mmps[jj] = 0.0; //stop linear path generation when linear path is within tolerance of target position.
dofydoink 5:712e7634c779 482 }
dofydoink 5:712e7634c779 483 mutPathIn.unlock();//unlock relevant mutex
dofydoink 0:607bc887b6e0 484
dofydoink 0:607bc887b6e0 485 //calculate next step in smooth path
dofydoink 5:712e7634c779 486 dblSmoothPathCurrentPos_mm[jj] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
dofydoink 5:712e7634c779 487
dofydoink 5:712e7634c779 488 mutChannel[jj].lock(); //MUTEX LOCK
dofydoink 5:712e7634c779 489 intDemandPos_Tx[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);//convert to a 13-bit number;
dofydoink 5:712e7634c779 490 intDemandPos_Tx[jj] = intDemandPos_Tx[jj] & 0x1FFF; //ensure number is 13-bit
dofydoink 5:712e7634c779 491 mutChannel[jj].unlock(); //MUTEX UNLOCK
dofydoink 5:712e7634c779 492 //if(jj == 0) printf("AFTER: %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[jj],dblVelocity_mmps[jj],dblMeasuredSampleTime);
dofydoink 0:607bc887b6e0 493
dofydoink 5:712e7634c779 494 isDataReady[jj] = 1;//signal that data ready
dofydoink 0:607bc887b6e0 495
WD40andTape 4:303584310071 496 } // end for
WD40andTape 6:f0a18e28a322 497 //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
WD40andTape 6:f0a18e28a322 498 /*printf("%f, %f, %f, %f, %f, %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblLinearPathCurrentPos_mm[1],dblLinearPathCurrentPos_mm[2],
WD40andTape 6:f0a18e28a322 499 dblLinearPathCurrentPos_mm[3],dblLinearPathCurrentPos_mm[4],dblLinearPathCurrentPos_mm[5],dblLinearPathCurrentPos_mm[6],dblLinearPathCurrentPos_mm[7]);*/
WD40andTape 6:f0a18e28a322 500 //printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
WD40andTape 4:303584310071 501 } // end while
WD40andTape 3:c83291bf9fd2 502 }
dofydoink 0:607bc887b6e0 503
dofydoink 0:607bc887b6e0 504
dofydoink 0:607bc887b6e0 505
dofydoink 0:607bc887b6e0 506 Ticker PathCalculationTicker;
dofydoink 0:607bc887b6e0 507
dofydoink 0:607bc887b6e0 508 int main()
dofydoink 0:607bc887b6e0 509 {
WD40andTape 4:303584310071 510 int ii;
WD40andTape 3:c83291bf9fd2 511
dofydoink 0:607bc887b6e0 512 //initialise relevant variables
dofydoink 0:607bc887b6e0 513 for(ii = 0; ii<N_CHANNELS; ii++)
dofydoink 0:607bc887b6e0 514 {
WD40andTape 3:c83291bf9fd2 515
dofydoink 0:607bc887b6e0 516 //all chip selects in off state
dofydoink 0:607bc887b6e0 517 cs_LL[ii] = 1;
dofydoink 0:607bc887b6e0 518 cs_ADC[ii] = 1;
dofydoink 0:607bc887b6e0 519
dofydoink 0:607bc887b6e0 520 //data ready flags set to not ready
WD40andTape 1:2a43cf183a62 521 isDataReady[ii] = 0;
dofydoink 0:607bc887b6e0 522 }
dofydoink 0:607bc887b6e0 523
dofydoink 0:607bc887b6e0 524 //calculate some control variables
WD40andTape 6:f0a18e28a322 525
dofydoink 0:607bc887b6e0 526
dofydoink 0:607bc887b6e0 527 pinReset = 1; //initialise reset pin to not reset the controllers.
dofydoink 5:712e7634c779 528 wait(0.25);
WD40andTape 3:c83291bf9fd2 529 pinReset=0; //reset controllers to be safe
dofydoink 5:712e7634c779 530 wait(0.25);
WD40andTape 3:c83291bf9fd2 531 pinReset = 1;//ready to go
dofydoink 0:607bc887b6e0 532
dofydoink 0:607bc887b6e0 533 //say something nice to the user.
WD40andTape 6:f0a18e28a322 534 pc.baud(9600); //115200
dofydoink 0:607bc887b6e0 535 printf("Hi, there! I'll be your mid-level controller for today.\r\n");
dofydoink 5:712e7634c779 536 wait(3);
dofydoink 0:607bc887b6e0 537 // Start the event queue
dofydoink 0:607bc887b6e0 538 t.start(callback(&queue, &EventQueue::dispatch_forever));
dofydoink 0:607bc887b6e0 539
dofydoink 0:607bc887b6e0 540 //set up the interrupts
dofydoink 0:607bc887b6e0 541
dofydoink 0:607bc887b6e0 542 //set up rise interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 3:c83291bf9fd2 543 //
WD40andTape 3:c83291bf9fd2 544 //pinGate[0].rise(&testRiseFunction);
WD40andTape 3:c83291bf9fd2 545 //testPinGate.rise(&testRiseFunction);//<working
WD40andTape 3:c83291bf9fd2 546 //testPinGate.rise(&rise0);
WD40andTape 3:c83291bf9fd2 547
WD40andTape 3:c83291bf9fd2 548 pinGate0.rise(&rise0);
dofydoink 5:712e7634c779 549
WD40andTape 3:c83291bf9fd2 550 pinGate1.rise(&rise1);
WD40andTape 3:c83291bf9fd2 551 pinGate2.rise(&rise2);
WD40andTape 3:c83291bf9fd2 552 pinGate3.rise(&rise3);
WD40andTape 3:c83291bf9fd2 553 pinGate4.rise(&rise4);
WD40andTape 3:c83291bf9fd2 554 pinGate5.rise(&rise5);
WD40andTape 3:c83291bf9fd2 555 pinGate6.rise(&rise6);
WD40andTape 3:c83291bf9fd2 556 pinGate7.rise(&rise7);
dofydoink 5:712e7634c779 557
dofydoink 0:607bc887b6e0 558
dofydoink 0:607bc887b6e0 559 //set up fall interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 3:c83291bf9fd2 560 //
WD40andTape 3:c83291bf9fd2 561 //pinGate[0].fall(&testFallFunction);
WD40andTape 3:c83291bf9fd2 562 //testPinGate.fall(&testFallFunction); <working
WD40andTape 3:c83291bf9fd2 563 //testPinGate.fall(&fall0);
WD40andTape 3:c83291bf9fd2 564
WD40andTape 3:c83291bf9fd2 565 pinGate0.fall(&fall0);
dofydoink 5:712e7634c779 566
WD40andTape 3:c83291bf9fd2 567 pinGate1.fall(&fall1);
WD40andTape 3:c83291bf9fd2 568 pinGate2.fall(&fall2);
WD40andTape 3:c83291bf9fd2 569 pinGate3.fall(&fall3);
WD40andTape 3:c83291bf9fd2 570 pinGate4.fall(&fall4);
WD40andTape 3:c83291bf9fd2 571 pinGate5.fall(&fall5);
WD40andTape 3:c83291bf9fd2 572 pinGate6.fall(&fall6);
WD40andTape 3:c83291bf9fd2 573 pinGate7.fall(&fall7);
dofydoink 5:712e7634c779 574
dofydoink 0:607bc887b6e0 575
dofydoink 0:607bc887b6e0 576 timer.start();
dofydoink 0:607bc887b6e0 577
WD40andTape 4:303584310071 578 //threadSimulateDemand.start(SimulateDemand);
WD40andTape 4:303584310071 579 threadReceiveAndReplan.start(ReceiveAndReplan);//start Replanning thread
WD40andTape 4:303584310071 580 //ReceiveAndReplan();
WD40andTape 4:303584310071 581 threadSmoothPathPlan.start(CalculateSmoothPath); //start planning thread
WD40andTape 1:2a43cf183a62 582
dofydoink 0:607bc887b6e0 583 PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); //set up planning thread to recur at fixed intervals
WD40andTape 1:2a43cf183a62 584
dofydoink 0:607bc887b6e0 585 Thread::wait(1);
WD40andTape 3:c83291bf9fd2 586
dofydoink 0:607bc887b6e0 587
WD40andTape 1:2a43cf183a62 588 while(1) {
WD40andTape 1:2a43cf183a62 589 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 590 }
dofydoink 0:607bc887b6e0 591 }
dofydoink 0:607bc887b6e0 592
WD40andTape 6:f0a18e28a322 593
WD40andTape 6:f0a18e28a322 594 int setup_server(void) {
WD40andTape 6:f0a18e28a322 595 int error_code;
WD40andTape 6:f0a18e28a322 596 printf("Starting TCP server...\n\r");
WD40andTape 6:f0a18e28a322 597 if( !IS_DHCP ) {
WD40andTape 6:f0a18e28a322 598 const char* ip = "10.101.79.135"; //"10.101.81.125"; // Get first 3 parts from "ifconfig" on PC
WD40andTape 6:f0a18e28a322 599 const char* mask = "255.255.255.0"; // Get from "ifconfig" on PC
WD40andTape 6:f0a18e28a322 600 const char* gateway = "10.101.79.1"; //"10.101.81.1"; // Get from "route -n" on PC
WD40andTape 6:f0a18e28a322 601 error_code = comms.eth.set_network(ip, mask, gateway); // Dont use DHCP
WD40andTape 6:f0a18e28a322 602 if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 603 printf("Error %i. Could not network interface to use a static IP address. "
WD40andTape 6:f0a18e28a322 604 "Perhaps the network is already connected?\n\r", error_code);
WD40andTape 6:f0a18e28a322 605 return -1;
WD40andTape 6:f0a18e28a322 606 }
WD40andTape 6:f0a18e28a322 607 }
WD40andTape 6:f0a18e28a322 608 error_code = comms.eth.connect();
WD40andTape 6:f0a18e28a322 609 if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 610 printf("Error %i. Could not start the Ethernet interface.\n\r", error_code);
WD40andTape 6:f0a18e28a322 611 return -1;
WD40andTape 6:f0a18e28a322 612 }
WD40andTape 6:f0a18e28a322 613 const char* server_ip = comms.eth.get_ip_address();
WD40andTape 6:f0a18e28a322 614 if( server_ip == NULL ) {
WD40andTape 6:f0a18e28a322 615 error_code = -1;
WD40andTape 6:f0a18e28a322 616 printf("Error %i. Ethernet interface is not yet connected.\n\r", error_code);
WD40andTape 6:f0a18e28a322 617 return -1;
WD40andTape 6:f0a18e28a322 618 }
WD40andTape 6:f0a18e28a322 619 printf("The target IP address is '%s'\n\r", server_ip);
WD40andTape 6:f0a18e28a322 620 error_code = comms.srv.open(&comms.eth); // Open the server on ethernet stack
WD40andTape 6:f0a18e28a322 621 if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 622 printf("Error %i. Could not open server socket.\n\r", error_code);
WD40andTape 6:f0a18e28a322 623 return -1;
WD40andTape 6:f0a18e28a322 624 }
WD40andTape 6:f0a18e28a322 625 error_code = comms.srv.bind(comms.eth.get_ip_address(), PORT); // Bind the HTTP port (TCP 80) to the server
WD40andTape 6:f0a18e28a322 626 if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 627 printf("Error %i. Could not bind server socket to port '%d'.\n\r", error_code, PORT);
WD40andTape 6:f0a18e28a322 628 return -1;
WD40andTape 6:f0a18e28a322 629 }
WD40andTape 6:f0a18e28a322 630 error_code = comms.srv.listen(1); // Listen for 1 simultaneous connection
WD40andTape 6:f0a18e28a322 631 if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 632 printf("Error %i. Could not put server socket into listening mode.\n\r", error_code);
WD40andTape 6:f0a18e28a322 633 return -1;
WD40andTape 6:f0a18e28a322 634 }
WD40andTape 6:f0a18e28a322 635 return 0;
WD40andTape 6:f0a18e28a322 636 }
WD40andTape 6:f0a18e28a322 637
WD40andTape 6:f0a18e28a322 638
WD40andTape 6:f0a18e28a322 639 int accept_connection(void) {
WD40andTape 6:f0a18e28a322 640 printf("Waiting to accept a connection on the TCP socket.\n\r");
WD40andTape 6:f0a18e28a322 641 int error_code;
WD40andTape 6:f0a18e28a322 642 error_code = comms.srv.accept(&comms.clt_sock, &comms.clt_addr); // Blocks until data is sent
WD40andTape 6:f0a18e28a322 643 if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 644 printf("Error %i. Could not create a network socket using the specified socket instance. "
WD40andTape 6:f0a18e28a322 645 "Perhaps the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 6:f0a18e28a322 646 return -1;
WD40andTape 6:f0a18e28a322 647 }
WD40andTape 6:f0a18e28a322 648 printf("Accepted %s:%d\n\r", comms.clt_addr.get_ip_address(), comms.clt_addr.get_port());
WD40andTape 6:f0a18e28a322 649 return 0;
WD40andTape 6:f0a18e28a322 650 }
WD40andTape 6:f0a18e28a322 651
WD40andTape 6:f0a18e28a322 652
WD40andTape 6:f0a18e28a322 653 void close_server(void) {
WD40andTape 6:f0a18e28a322 654 comms.clt_sock.close();
WD40andTape 6:f0a18e28a322 655 comms.srv.close();
WD40andTape 6:f0a18e28a322 656 comms.eth.disconnect();
WD40andTape 6:f0a18e28a322 657 return;
WD40andTape 6:f0a18e28a322 658 }
WD40andTape 6:f0a18e28a322 659
WD40andTape 6:f0a18e28a322 660
WD40andTape 6:f0a18e28a322 661 msg_format consume_message( unsigned char * msg ) {
WD40andTape 6:f0a18e28a322 662 // Break message string into chunks at each comma
WD40andTape 6:f0a18e28a322 663 vector<string> tokens;
WD40andTape 6:f0a18e28a322 664 char * delim = ",";
WD40andTape 6:f0a18e28a322 665 char * token = strtok((char *)msg, delim);
WD40andTape 6:f0a18e28a322 666 while(token != NULL) {
WD40andTape 6:f0a18e28a322 667 tokens.push_back(string(token));
WD40andTape 6:f0a18e28a322 668 //printf("%s\n\r",token);
WD40andTape 6:f0a18e28a322 669 token = strtok(NULL, delim);
WD40andTape 6:f0a18e28a322 670 }
WD40andTape 6:f0a18e28a322 671 // Cast into doubles and assign to variables
WD40andTape 6:f0a18e28a322 672 struct msg_format input;
WD40andTape 6:f0a18e28a322 673 stringstream(tokens[0]) >> input.psi[0][0];
WD40andTape 6:f0a18e28a322 674 stringstream(tokens[1]) >> input.psi[0][1];
WD40andTape 6:f0a18e28a322 675 stringstream(tokens[2]) >> input.psi[0][2];
WD40andTape 6:f0a18e28a322 676 stringstream(tokens[3]) >> input.psi[1][0];
WD40andTape 6:f0a18e28a322 677 stringstream(tokens[4]) >> input.psi[1][1];
WD40andTape 6:f0a18e28a322 678 stringstream(tokens[5]) >> input.psi[1][2];
WD40andTape 6:f0a18e28a322 679 stringstream(tokens[6]) >> input.psi[2][0];
WD40andTape 6:f0a18e28a322 680 stringstream(tokens[7]) >> input.psi[2][1];
WD40andTape 6:f0a18e28a322 681 stringstream(tokens[8]) >> input.psi[2][2];
WD40andTape 6:f0a18e28a322 682 stringstream(tokens[9]) >> input.duration;
WD40andTape 6:f0a18e28a322 683
WD40andTape 6:f0a18e28a322 684 if(input.psi[0][0] < 0.0) input.psi[0][0] = 0.0;
WD40andTape 6:f0a18e28a322 685 if(input.psi[0][1] < 0.0) input.psi[0][1] = 0.0;
WD40andTape 6:f0a18e28a322 686 if(input.psi[0][2] < 0.0) input.psi[0][2] = 0.0;
WD40andTape 6:f0a18e28a322 687 if(input.psi[1][0] < 0.0) input.psi[1][0] = 0.0;
WD40andTape 6:f0a18e28a322 688 if(input.psi[1][1] < 0.0) input.psi[1][1] = 0.0;
WD40andTape 6:f0a18e28a322 689 if(input.psi[1][2] < 0.0) input.psi[1][2] = 0.0;
WD40andTape 6:f0a18e28a322 690 if(input.psi[2][0] < 0.0) input.psi[2][0] = 0.0;
WD40andTape 6:f0a18e28a322 691 if(input.psi[2][1] < 0.0) input.psi[2][1] = 0.0;
WD40andTape 6:f0a18e28a322 692 if(input.psi[2][2] < 0.0) input.psi[2][2] = 0.0;
WD40andTape 6:f0a18e28a322 693 if(input.duration < 0.0) input.duration = 0.0;
WD40andTape 6:f0a18e28a322 694 /*if( input.psi[2][0] > 0.14 ) {
WD40andTape 6:f0a18e28a322 695 printf("Hi\n\r");
WD40andTape 6:f0a18e28a322 696 }*/
WD40andTape 6:f0a18e28a322 697 //throw std::invalid_argument( "received negative value" );
WD40andTape 6:f0a18e28a322 698 //return 0;
WD40andTape 6:f0a18e28a322 699 return input;
WD40andTape 6:f0a18e28a322 700 } // End of consume_message()