MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

main.cpp@5:712e7634c779, 2018-08-01 (annotated)

Committer:
dofydoink
Date:
Wed Aug 01 09:51:23 2018 +0000
Revision:
5:712e7634c779
Parent:
4:303584310071
Child:
6:f0a18e28a322
Ttested

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"
dofydoink 0:607bc887b6e0 3
dofydoink 0:607bc887b6e0 4 //#include "mbed.h"
dofydoink 0:607bc887b6e0 5 #include "mbed_events.h"
dofydoink 0:607bc887b6e0 6
dofydoink 0:607bc887b6e0 7 //ADC SPI stuff
dofydoink 0:607bc887b6e0 8 #define PREAMBLE 0x06
dofydoink 0:607bc887b6e0 9 #define CHAN_1 0x30
dofydoink 0:607bc887b6e0 10 #define CHAN_2 0x70
dofydoink 0:607bc887b6e0 11 #define CHAN_3 0xB0
dofydoink 0:607bc887b6e0 12 #define CHAN_4 0xF0
dofydoink 0:607bc887b6e0 13
dofydoink 0:607bc887b6e0 14 #define DATA_MASK 0x0F
dofydoink 0:607bc887b6e0 15
WD40andTape 1:2a43cf183a62 16 //simple channel selection
dofydoink 0:607bc887b6e0 17 #define ADC_PRESSURE 1
dofydoink 0:607bc887b6e0 18 #define ADC_POSITION 3
WD40andTape 1:2a43cf183a62 19 //---------------------
dofydoink 0:607bc887b6e0 20
dofydoink 0:607bc887b6e0 21 #define N_CHANNELS 8 //number of channels to control
dofydoink 0:607bc887b6e0 22 //1-3: front segment
dofydoink 0:607bc887b6e0 23 //4-6: rear segment
dofydoink 0:607bc887b6e0 24 //7-8: mid segment
dofydoink 0:607bc887b6e0 25
dofydoink 0:607bc887b6e0 26 //parameters to manually change
dofydoink 0:607bc887b6e0 27 #define LOW_LEVEL_SPI_FREQUENCY 10000000
dofydoink 0:607bc887b6e0 28
WD40andTape 4:303584310071 29 #define PATH_SAMPLE_TIME_S 0.005
dofydoink 0:607bc887b6e0 30
dofydoink 0:607bc887b6e0 31 #define MAX_LENGTH_MM 100.0
WD40andTape 3:c83291bf9fd2 32 #define MAX_ACTUATOR_LENGTH 52.2
dofydoink 0:607bc887b6e0 33 #define MAX_SPEED_MMPS 24.3457
WD40andTape 4:303584310071 34 #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 35
dofydoink 5:712e7634c779 36 const double DBL_MAX_CHAMBER_LENGTHS_MM[N_CHANNELS] = {500.0,50.0,50.0,50.0,50.0,50.0,30.0,30.0};
WD40andTape 4:303584310071 37 const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {0.24176,0.24176,0.24176,0.24176,0.24176,0.36264,0.36264};// covert from chamber lengths to actuator lengths
WD40andTape 4:303584310071 38 const double DBL_SMOOTHING_FACTOR = 0.5; // 0<x<=1, where 1 is no smoothing
WD40andTape 4:303584310071 39 const double DBL_PATH_TOLERANCE = 0.2;
WD40andTape 4:303584310071 40 //-----These are the variables being shared between High-Level and Mid-Level-----------------------------------------------//
dofydoink 0:607bc887b6e0 41
dofydoink 0:607bc887b6e0 42 //
dofydoink 0:607bc887b6e0 43 Semaphore semReplan(1);// this must be set every time new high-level transmissions are received to allow replanning to take place!
dofydoink 0:607bc887b6e0 44 //--------------------------------------------------------------------------------------------------------------------------//
dofydoink 0:607bc887b6e0 45
dofydoink 0:607bc887b6e0 46 //boolean flag to indicate that new target information has been received
dofydoink 0:607bc887b6e0 47 //bool blnReplan;//set this when new transmission is received over ethernet
dofydoink 0:607bc887b6e0 48
dofydoink 0:607bc887b6e0 49 //path variables
WD40andTape 4:303584310071 50 double dblVelocity_mmps[N_CHANNELS] = { 0.0 };//the linear path velocity (not sent to actuator) KEEP GLOBAL
WD40andTape 4:303584310071 51 double dblLinearPathCurrentPos_mm[N_CHANNELS]={ 0.0 }; //the current position of the linear path (not sent to actuator) //KEEP GLOBAL
WD40andTape 4:303584310071 52 double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; //The final target position for the actuator KEEP GLOBAL
dofydoink 0:607bc887b6e0 53
WD40andTape 4:303584310071 54 int intDemandPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator KEEP GLOBAL
dofydoink 0:607bc887b6e0 55
WD40andTape 4:303584310071 56 char chrErrorFlag[N_CHANNELS];// 3 error bits from LL KEEP GLOBAL
dofydoink 0:607bc887b6e0 57
WD40andTape 4:303584310071 58 //pin declarations----------------
dofydoink 0:607bc887b6e0 59 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
dofydoink 0:607bc887b6e0 60
WD40andTape 3:c83291bf9fd2 61 InterruptIn pinGate6(PE_11); //this pin HAS TO BE defined before SPI set up. No Clue Why.
WD40andTape 3:c83291bf9fd2 62
WD40andTape 3:c83291bf9fd2 63 SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
WD40andTape 3:c83291bf9fd2 64
WD40andTape 3:c83291bf9fd2 65 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 66 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 67
WD40andTape 3:c83291bf9fd2 68 DigitalOut pinCheck(PE_5);
WD40andTape 3:c83291bf9fd2 69
WD40andTape 3:c83291bf9fd2 70 //these interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
WD40andTape 3:c83291bf9fd2 71 InterruptIn pinGate0(PF_11);
WD40andTape 3:c83291bf9fd2 72 InterruptIn pinGate1(PG_14);
WD40andTape 3:c83291bf9fd2 73 InterruptIn pinGate2(PF_15);
WD40andTape 3:c83291bf9fd2 74 InterruptIn pinGate3(PF_12);
WD40andTape 3:c83291bf9fd2 75 InterruptIn pinGate4(PF_3);
WD40andTape 3:c83291bf9fd2 76 InterruptIn pinGate5(PF_13);
WD40andTape 4:303584310071 77 //InterruptIn pinGate6(PE_11); //see above nonsense
WD40andTape 3:c83291bf9fd2 78 InterruptIn pinGate7(PE_13);
dofydoink 0:607bc887b6e0 79
dofydoink 0:607bc887b6e0 80 DigitalOut pinReset(PD_2); //reset pin for all controllers.
dofydoink 0:607bc887b6e0 81
WD40andTape 3:c83291bf9fd2 82
dofydoink 0:607bc887b6e0 83 EventQueue queue(32 * EVENTS_EVENT_SIZE);
dofydoink 0:607bc887b6e0 84
dofydoink 0:607bc887b6e0 85 Thread t(osPriorityRealtime);
dofydoink 0:607bc887b6e0 86
WD40andTape 4:303584310071 87 Thread threadReceiveAndReplan(osPriorityBelowNormal);
WD40andTape 4:303584310071 88 Thread threadSmoothPathPlan(osPriorityNormal);
dofydoink 0:607bc887b6e0 89
dofydoink 0:607bc887b6e0 90 Thread threadSimulateDemand(osPriorityHigh);
dofydoink 0:607bc887b6e0 91
WD40andTape 1:2a43cf183a62 92 Mutex mutChannel[N_CHANNELS];
WD40andTape 4:303584310071 93 Mutex mutPathIn;
dofydoink 0:607bc887b6e0 94
dofydoink 0:607bc887b6e0 95 Semaphore semPathPlan(1);//
dofydoink 0:607bc887b6e0 96
dofydoink 0:607bc887b6e0 97 Timer timer;//timers are nice
dofydoink 0:607bc887b6e0 98
dofydoink 0:607bc887b6e0 99 int ThreadID[N_CHANNELS];
dofydoink 0:607bc887b6e0 100
WD40andTape 1:2a43cf183a62 101 bool isDataReady[N_CHANNELS]; // flag to indicate path data is ready for transmission to low level.
dofydoink 0:607bc887b6e0 102
dofydoink 5:712e7634c779 103 double dblGlobalTest;
dofydoink 5:712e7634c779 104 int intGlobalTest;
WD40andTape 4:303584310071 105 double ReadADCPosition_mtrs(int channel)
dofydoink 0:607bc887b6e0 106 {
dofydoink 0:607bc887b6e0 107 unsigned int outputA;
dofydoink 0:607bc887b6e0 108 unsigned int outputB;
WD40andTape 3:c83291bf9fd2 109 int output;
WD40andTape 3:c83291bf9fd2 110 double dblOutput;
dofydoink 0:607bc887b6e0 111
dofydoink 0:607bc887b6e0 112 spi.format(8,0);
dofydoink 0:607bc887b6e0 113 spi.frequency(1000000);
dofydoink 0:607bc887b6e0 114
dofydoink 0:607bc887b6e0 115 cs_ADC[channel] = 0;
dofydoink 0:607bc887b6e0 116 spi.write(PREAMBLE);
dofydoink 0:607bc887b6e0 117 outputA = spi.write(CHAN_3);
dofydoink 0:607bc887b6e0 118 outputB = spi.write(0xFF);
WD40andTape 1:2a43cf183a62 119 cs_ADC[channel]= 1;
dofydoink 0:607bc887b6e0 120
dofydoink 0:607bc887b6e0 121 outputA = outputA & DATA_MASK;
dofydoink 0:607bc887b6e0 122 outputA = outputA<<8;
dofydoink 0:607bc887b6e0 123 output = (outputA | outputB);
WD40andTape 3:c83291bf9fd2 124 output = 4095- output;
dofydoink 5:712e7634c779 125 dblOutput = (double) (output);
dofydoink 5:712e7634c779 126 dblOutput = dblOutput*0.0229 - 21.582;
dofydoink 5:712e7634c779 127 //dblOutput = dblOutput - 21.78;
WD40andTape 3:c83291bf9fd2 128 return dblOutput;
dofydoink 5:712e7634c779 129 //dblOutput = (double) ((output/4095)*100.0);
dofydoink 5:712e7634c779 130 // dblOutput = dblOutput - 36.727717;
dofydoink 5:712e7634c779 131 // return dblOutput;
dofydoink 0:607bc887b6e0 132 }
dofydoink 0:607bc887b6e0 133
WD40andTape 4:303584310071 134 double ReadADCPressure_bar(int channel)
dofydoink 0:607bc887b6e0 135 {
dofydoink 0:607bc887b6e0 136 unsigned int outputA;
dofydoink 0:607bc887b6e0 137 unsigned int outputB;
WD40andTape 3:c83291bf9fd2 138 int output;
WD40andTape 3:c83291bf9fd2 139 double dblOutput;
dofydoink 0:607bc887b6e0 140
dofydoink 0:607bc887b6e0 141 spi.format(8,0);
dofydoink 0:607bc887b6e0 142 spi.frequency(1000000);
dofydoink 0:607bc887b6e0 143
dofydoink 0:607bc887b6e0 144 cs_ADC[channel] = 0;
dofydoink 0:607bc887b6e0 145 spi.write(PREAMBLE);
dofydoink 0:607bc887b6e0 146 outputA = spi.write(CHAN_1);
dofydoink 0:607bc887b6e0 147 outputB = spi.write(0xFF);
dofydoink 0:607bc887b6e0 148 cs_ADC[channel] = 1;
dofydoink 0:607bc887b6e0 149
dofydoink 0:607bc887b6e0 150 outputA = outputA & DATA_MASK;
dofydoink 0:607bc887b6e0 151 outputA = outputA<<8;
dofydoink 0:607bc887b6e0 152 output = (outputA | outputB);
dofydoink 0:607bc887b6e0 153
dofydoink 5:712e7634c779 154 dblOutput = (double)(output);
dofydoink 5:712e7634c779 155 dblOutput = dblOutput-502.0;
dofydoink 5:712e7634c779 156 dblOutput = dblOutput/4095.0*8.0;
WD40andTape 3:c83291bf9fd2 157 return dblOutput;
dofydoink 0:607bc887b6e0 158 }
dofydoink 0:607bc887b6e0 159
WD40andTape 1:2a43cf183a62 160 void SendReceiveData(int channel)
dofydoink 0:607bc887b6e0 161 {
WD40andTape 3:c83291bf9fd2 162
WD40andTape 4:303584310071 163
WD40andTape 4:303584310071 164 int intPosSPI_Rx[N_CHANNELS]; //13 bit value received over SPI from the actuator
WD40andTape 4:303584310071 165
WD40andTape 4:303584310071 166
dofydoink 0:607bc887b6e0 167 //get data from controller
dofydoink 0:607bc887b6e0 168 spi.format(16,2);
dofydoink 0:607bc887b6e0 169 spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 4:303584310071 170 mutChannel[channel].lock();//lock mutex for specific Channel
dofydoink 0:607bc887b6e0 171 cs_LL[channel] = 0;//select relevant chip
WD40andTape 1:2a43cf183a62 172 intPosSPI_Rx[channel] = spi.write(intDemandPos_Tx[channel]); //transmit & receive
dofydoink 0:607bc887b6e0 173 cs_LL[channel] = 1;//deselect chip
WD40andTape 1:2a43cf183a62 174 isDataReady[channel] = 0;//data no longer ready, i.e. we now require new data
dofydoink 5:712e7634c779 175 if(channel == 0) {
dofydoink 5:712e7634c779 176 intGlobalTest = intPosSPI_Rx[channel];
dofydoink 5:712e7634c779 177 dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
dofydoink 5:712e7634c779 178 }
WD40andTape 1:2a43cf183a62 179
dofydoink 0:607bc887b6e0 180
dofydoink 0:607bc887b6e0 181 //sort out received data
dofydoink 0:607bc887b6e0 182 chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
WD40andTape 4:303584310071 183
dofydoink 0:607bc887b6e0 184 intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
WD40andTape 4:303584310071 185 //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
WD40andTape 4:303584310071 186 mutChannel[channel].unlock();//unlock mutex for specific channel
WD40andTape 3:c83291bf9fd2 187 //printf("%d, %d\r\n",intPosSPI_Rx[0],intDemandPos_Tx[0]);
WD40andTape 3:c83291bf9fd2 188
WD40andTape 3:c83291bf9fd2 189 }
WD40andTape 3:c83291bf9fd2 190
WD40andTape 4:303584310071 191
dofydoink 0:607bc887b6e0 192
dofydoink 0:607bc887b6e0 193 //common rise handler function
dofydoink 0:607bc887b6e0 194
dofydoink 0:607bc887b6e0 195 void common_rise_handler(int channel)
dofydoink 0:607bc887b6e0 196 {
WD40andTape 3:c83291bf9fd2 197 pinCheck = 1;
dofydoink 0:607bc887b6e0 198 //check if data is ready for tranmission
WD40andTape 1:2a43cf183a62 199 if (isDataReady[channel])
dofydoink 0:607bc887b6e0 200 {
dofydoink 0:607bc887b6e0 201 // Add transmit task to event queue
WD40andTape 1:2a43cf183a62 202 ThreadID[channel] = queue.call(SendReceiveData,channel);//schedule transmission
dofydoink 0:607bc887b6e0 203 }
dofydoink 0:607bc887b6e0 204 }
dofydoink 0:607bc887b6e0 205
WD40andTape 3:c83291bf9fd2 206
dofydoink 0:607bc887b6e0 207
dofydoink 0:607bc887b6e0 208
dofydoink 0:607bc887b6e0 209 //common_fall handler functions
dofydoink 0:607bc887b6e0 210 void common_fall_handler(int channel)
dofydoink 0:607bc887b6e0 211 {
WD40andTape 3:c83291bf9fd2 212 pinCheck = 0;
dofydoink 0:607bc887b6e0 213 //cancel relevant queued event
dofydoink 0:607bc887b6e0 214 queue.cancel(ThreadID[channel]);
dofydoink 0:607bc887b6e0 215 }
dofydoink 0:607bc887b6e0 216
dofydoink 0:607bc887b6e0 217 //stub rise functions
dofydoink 0:607bc887b6e0 218 void rise0(void) { common_rise_handler(0); }
dofydoink 0:607bc887b6e0 219 void rise1(void) { common_rise_handler(1); }
dofydoink 0:607bc887b6e0 220 void rise2(void) { common_rise_handler(2); }
dofydoink 0:607bc887b6e0 221 void rise3(void) { common_rise_handler(3); }
dofydoink 0:607bc887b6e0 222 void rise4(void) { common_rise_handler(4); }
dofydoink 0:607bc887b6e0 223 void rise5(void) { common_rise_handler(5); }
dofydoink 0:607bc887b6e0 224 void rise6(void) { common_rise_handler(6); }
dofydoink 0:607bc887b6e0 225 void rise7(void) { common_rise_handler(7); }
dofydoink 0:607bc887b6e0 226
dofydoink 0:607bc887b6e0 227 //stub fall functions
dofydoink 0:607bc887b6e0 228 void fall0(void) { common_fall_handler(0); }
dofydoink 0:607bc887b6e0 229 void fall1(void) { common_fall_handler(1); }
dofydoink 0:607bc887b6e0 230 void fall2(void) { common_fall_handler(2); }
dofydoink 0:607bc887b6e0 231 void fall3(void) { common_fall_handler(3); }
dofydoink 0:607bc887b6e0 232 void fall4(void) { common_fall_handler(4); }
dofydoink 0:607bc887b6e0 233 void fall5(void) { common_fall_handler(5); }
dofydoink 0:607bc887b6e0 234 void fall6(void) { common_fall_handler(6); }
dofydoink 0:607bc887b6e0 235 void fall7(void) { common_fall_handler(7); }
dofydoink 0:607bc887b6e0 236
WD40andTape 1:2a43cf183a62 237 void startPathPlan() // Plan a new linear path after receiving new target data
dofydoink 0:607bc887b6e0 238 {
WD40andTape 4:303584310071 239 semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
dofydoink 0:607bc887b6e0 240 }
dofydoink 0:607bc887b6e0 241
dofydoink 5:712e7634c779 242 double somedumbthing;
dofydoink 5:712e7634c779 243
dofydoink 0:607bc887b6e0 244 //this function will be called when a new transmission is received from high level
WD40andTape 4:303584310071 245 void ReceiveAndReplan()
dofydoink 0:607bc887b6e0 246 {
WD40andTape 4:303584310071 247 int ii;
WD40andTape 4:303584310071 248
WD40andTape 4:303584310071 249 double dblPSI[3][3]; //the message from high-level containing the chamber lengths in meters in following format:
WD40andTape 4:303584310071 250 /*
WD40andTape 4:303584310071 251 {L(front,1), L(front,2), L(front,3);
WD40andTape 4:303584310071 252 L(mid,1) , L(mid,2) , L(mid,3);
WD40andTape 4:303584310071 253 L(rear,1) , L(rear,2) , L(rear,3);}
WD40andTape 4:303584310071 254 */
WD40andTape 4:303584310071 255 double dblTargetTime_s; //the time in seconds desired to reach the target (>0...!)
dofydoink 5:712e7634c779 256 //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 257 // double dblPressure_bar[N_CHANNELS]; //the pressure in a given chamber in bar (1 bar = 100,000 Pa).
dofydoink 5:712e7634c779 258
WD40andTape 4:303584310071 259
WD40andTape 4:303584310071 260 //!!!!!!!!!!!!!!! If received messages faster than path replanning, will get increasingly behind time --> need to throw away some instructions
WD40andTape 2:eea12b149dba 261
WD40andTape 4:303584310071 262 //receive
WD40andTape 4:303584310071 263
WD40andTape 4:303584310071 264 double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
WD40andTape 4:303584310071 265
WD40andTape 4:303584310071 266 bool kk = 0;
WD40andTape 4:303584310071 267 while(1) {
WD40andTape 1:2a43cf183a62 268
WD40andTape 4:303584310071 269 if(kk == 0) {
dofydoink 5:712e7634c779 270 dblPSI[0][0] = (double) 0.20;
dofydoink 5:712e7634c779 271 dblTargetTime_s = 3.0;
WD40andTape 4:303584310071 272 } else {
WD40andTape 4:303584310071 273 dblPSI[0][0] = (double) 0.0;
dofydoink 5:712e7634c779 274 dblTargetTime_s = 3.0;
WD40andTape 4:303584310071 275 }
WD40andTape 4:303584310071 276 kk = !kk;
WD40andTape 4:303584310071 277 printf("REPLAN, %f,\r\n", dblPSI[0][0] );
dofydoink 0:607bc887b6e0 278 //update front segment
dofydoink 0:607bc887b6e0 279 dblTargetChambLen_mm[0] = dblPSI[0][0]*1000;
dofydoink 0:607bc887b6e0 280 dblTargetChambLen_mm[1] = dblPSI[0][1]*1000;
dofydoink 0:607bc887b6e0 281 dblTargetChambLen_mm[2] = dblPSI[0][2]*1000;
dofydoink 0:607bc887b6e0 282 //update mid segment
dofydoink 0:607bc887b6e0 283 dblTargetChambLen_mm[6] = dblPSI[1][0]*1000;
dofydoink 0:607bc887b6e0 284 dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; //same because two pumps are used
dofydoink 0:607bc887b6e0 285 //update rear segment
dofydoink 0:607bc887b6e0 286 dblTargetChambLen_mm[3] = dblPSI[2][0]*1000;
dofydoink 0:607bc887b6e0 287 dblTargetChambLen_mm[4] = dblPSI[2][1]*1000;
dofydoink 0:607bc887b6e0 288 dblTargetChambLen_mm[5] = dblPSI[2][2]*1000;
dofydoink 0:607bc887b6e0 289
WD40andTape 4:303584310071 290 for (ii = 0; ii< N_CHANNELS; ii++) {
dofydoink 0:607bc887b6e0 291 //check to see if positions are achievable
WD40andTape 4:303584310071 292 if(dblTargetChambLen_mm[ii]>DBL_MAX_CHAMBER_LENGTHS_MM[ii]) {
WD40andTape 4:303584310071 293 dblTargetChambLen_mm[ii] = DBL_MAX_CHAMBER_LENGTHS_MM[ii];
dofydoink 0:607bc887b6e0 294 }
WD40andTape 4:303584310071 295 if(dblTargetChambLen_mm[ii]<0.0) {
dofydoink 0:607bc887b6e0 296 dblTargetChambLen_mm[ii] = 0.0;
dofydoink 0:607bc887b6e0 297 }
dofydoink 0:607bc887b6e0 298
dofydoink 5:712e7634c779 299 mutPathIn.lock();//lock variables to avoid race condition
dofydoink 5:712e7634c779 300 dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
dofydoink 5:712e7634c779 301
dofydoink 5:712e7634c779 302 //work out new velocities
dofydoink 5:712e7634c779 303 dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblTargetTime_s;
dofydoink 5:712e7634c779 304
dofydoink 5:712e7634c779 305 //check to see if velocities are achievable
dofydoink 5:712e7634c779 306 if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
dofydoink 5:712e7634c779 307 if(dblVelocity_mmps[ii]>0) {
dofydoink 5:712e7634c779 308 dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
dofydoink 5:712e7634c779 309 } else {
dofydoink 5:712e7634c779 310 dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
dofydoink 5:712e7634c779 311 }
dofydoink 5:712e7634c779 312 }
dofydoink 5:712e7634c779 313 mutPathIn.unlock(); //unlock mutex.
dofydoink 0:607bc887b6e0 314
dofydoink 0:607bc887b6e0 315
dofydoink 5:712e7634c779 316 //dblPressure_bar[ii] = 0.5;//read pressure from channel
dofydoink 5:712e7634c779 317 // dblPosition_mtrs[ii] = 0.02; //read position from channel
WD40andTape 4:303584310071 318
WD40andTape 4:303584310071 319 //send info back to HL
WD40andTape 4:303584310071 320 } // end of for
WD40andTape 4:303584310071 321 //printf("%f\r\n", dblVelocity_mmps[0]);
WD40andTape 4:303584310071 322 Thread::wait(7000);
WD40andTape 4:303584310071 323 } // end of while(1)
dofydoink 0:607bc887b6e0 324 }
dofydoink 0:607bc887b6e0 325
WD40andTape 2:eea12b149dba 326 // lock mutex
WD40andTape 2:eea12b149dba 327 // get variables
WD40andTape 2:eea12b149dba 328 // unlock mutex
WD40andTape 2:eea12b149dba 329 // do calculations
WD40andTape 2:eea12b149dba 330 // lock mutex
WD40andTape 2:eea12b149dba 331 // set variables
WD40andTape 4:303584310071 332 // unlock mutex
WD40andTape 2:eea12b149dba 333
WD40andTape 4:303584310071 334 void CalculateSmoothPath() {
dofydoink 5:712e7634c779 335 int jj;
WD40andTape 1:2a43cf183a62 336 double dblMeasuredSampleTime;
WD40andTape 4:303584310071 337 double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; //the current position of the smooth path (not sent to actuator)
dofydoink 5:712e7634c779 338 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 339 double dblPressure_bar[N_CHANNELS]; //the pressure in a given chamber in bar (1 bar = 100,000 Pa).
WD40andTape 4:303584310071 340
WD40andTape 4:303584310071 341 while(1) {
dofydoink 0:607bc887b6e0 342 semPathPlan.wait();
WD40andTape 1:2a43cf183a62 343
WD40andTape 1:2a43cf183a62 344 // If run time is more than 50 us from expected, calculate from measured time step
WD40andTape 4:303584310071 345 if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50) {
dofydoink 5:712e7634c779 346 dblMeasuredSampleTime = timer.read();
WD40andTape 4:303584310071 347 } else {
WD40andTape 1:2a43cf183a62 348 dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
WD40andTape 1:2a43cf183a62 349 }
WD40andTape 4:303584310071 350 timer.reset();
WD40andTape 1:2a43cf183a62 351
dofydoink 5:712e7634c779 352 for(jj = 0; jj < N_CHANNELS; jj++) {
dofydoink 5:712e7634c779 353
dofydoink 5:712e7634c779 354 dblPressure_bar[jj] = ReadADCPressure_bar(jj);//read pressure from channel
dofydoink 5:712e7634c779 355 dblPosition_mtrs[jj] = ReadADCPosition_mtrs(jj); //read position from channel
dofydoink 5:712e7634c779 356 //DEBUGGING
dofydoink 5:712e7634c779 357 //if(jj == 0) printf("BEFORE: %f, %f, %f\r\n",dblVelocity_mmps[0],dblPosition_mtrs[0], dblMeasuredSampleTime);
WD40andTape 1:2a43cf183a62 358
dofydoink 5:712e7634c779 359 //calculate next step in linear path
dofydoink 5:712e7634c779 360 mutPathIn.lock();//lock relevant mutex
dofydoink 5:712e7634c779 361 dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime; // PATH_SAMPLE_TIME_S SHOULD BE MEASURED
dofydoink 5:712e7634c779 362 if(dblLinearPathCurrentPos_mm[jj] < 0.0) {
dofydoink 5:712e7634c779 363 dblLinearPathCurrentPos_mm[jj] = 0.00;
dofydoink 5:712e7634c779 364 }
dofydoink 5:712e7634c779 365
dofydoink 5:712e7634c779 366 //check tolerance
dofydoink 5:712e7634c779 367 if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= DBL_PATH_TOLERANCE) {
dofydoink 5:712e7634c779 368 dblVelocity_mmps[jj] = 0.0; //stop linear path generation when linear path is within tolerance of target position.
dofydoink 5:712e7634c779 369 }
dofydoink 5:712e7634c779 370 mutPathIn.unlock();//unlock relevant mutex
dofydoink 0:607bc887b6e0 371
dofydoink 0:607bc887b6e0 372 //calculate next step in smooth path
dofydoink 5:712e7634c779 373 dblSmoothPathCurrentPos_mm[jj] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
dofydoink 5:712e7634c779 374
dofydoink 5:712e7634c779 375 mutChannel[jj].lock(); //MUTEX LOCK
dofydoink 5:712e7634c779 376 intDemandPos_Tx[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);//convert to a 13-bit number;
dofydoink 5:712e7634c779 377 intDemandPos_Tx[jj] = intDemandPos_Tx[jj] & 0x1FFF; //ensure number is 13-bit
dofydoink 5:712e7634c779 378 mutChannel[jj].unlock(); //MUTEX UNLOCK
dofydoink 5:712e7634c779 379 //if(jj == 0) printf("AFTER: %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[jj],dblVelocity_mmps[jj],dblMeasuredSampleTime);
dofydoink 0:607bc887b6e0 380
dofydoink 5:712e7634c779 381 isDataReady[jj] = 1;//signal that data ready
dofydoink 0:607bc887b6e0 382
WD40andTape 4:303584310071 383 } // end for
WD40andTape 4:303584310071 384 //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
dofydoink 5:712e7634c779 385 mutChannel[0].lock();
dofydoink 5:712e7634c779 386 printf("%f, %f, %f\r\n",dblSmoothPathCurrentPos_mm[0],dblPressure_bar[0], dblPosition_mtrs[0]);
dofydoink 5:712e7634c779 387 mutChannel[0].unlock();
WD40andTape 4:303584310071 388 } // end while
WD40andTape 3:c83291bf9fd2 389 }
dofydoink 0:607bc887b6e0 390
dofydoink 0:607bc887b6e0 391
dofydoink 0:607bc887b6e0 392
dofydoink 0:607bc887b6e0 393 Ticker PathCalculationTicker;
dofydoink 0:607bc887b6e0 394
dofydoink 0:607bc887b6e0 395 int main()
dofydoink 0:607bc887b6e0 396 {
WD40andTape 4:303584310071 397 int ii;
WD40andTape 3:c83291bf9fd2 398
dofydoink 0:607bc887b6e0 399 //initialise relevant variables
dofydoink 0:607bc887b6e0 400 for(ii = 0; ii<N_CHANNELS; ii++)
dofydoink 0:607bc887b6e0 401 {
WD40andTape 3:c83291bf9fd2 402
dofydoink 0:607bc887b6e0 403 //all chip selects in off state
dofydoink 0:607bc887b6e0 404 cs_LL[ii] = 1;
dofydoink 0:607bc887b6e0 405 cs_ADC[ii] = 1;
dofydoink 0:607bc887b6e0 406
dofydoink 0:607bc887b6e0 407 //data ready flags set to not ready
WD40andTape 1:2a43cf183a62 408 isDataReady[ii] = 0;
dofydoink 0:607bc887b6e0 409 }
dofydoink 0:607bc887b6e0 410
dofydoink 0:607bc887b6e0 411 //calculate some control variables
WD40andTape 4:303584310071 412 //dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance in mm.
dofydoink 0:607bc887b6e0 413
dofydoink 0:607bc887b6e0 414 pinReset = 1; //initialise reset pin to not reset the controllers.
dofydoink 5:712e7634c779 415 wait(0.25);
WD40andTape 3:c83291bf9fd2 416 pinReset=0; //reset controllers to be safe
dofydoink 5:712e7634c779 417 wait(0.25);
WD40andTape 3:c83291bf9fd2 418 pinReset = 1;//ready to go
dofydoink 0:607bc887b6e0 419
dofydoink 0:607bc887b6e0 420 //say something nice to the user.
dofydoink 5:712e7634c779 421 pc.baud(115200);
dofydoink 0:607bc887b6e0 422 printf("Hi, there! I'll be your mid-level controller for today.\r\n");
dofydoink 5:712e7634c779 423 wait(3);
dofydoink 0:607bc887b6e0 424 // Start the event queue
dofydoink 0:607bc887b6e0 425 t.start(callback(&queue, &EventQueue::dispatch_forever));
dofydoink 0:607bc887b6e0 426
dofydoink 0:607bc887b6e0 427 //set up the interrupts
dofydoink 0:607bc887b6e0 428
dofydoink 0:607bc887b6e0 429 //set up rise interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 3:c83291bf9fd2 430 //
WD40andTape 3:c83291bf9fd2 431 //pinGate[0].rise(&testRiseFunction);
WD40andTape 3:c83291bf9fd2 432 //testPinGate.rise(&testRiseFunction);//<working
WD40andTape 3:c83291bf9fd2 433 //testPinGate.rise(&rise0);
WD40andTape 3:c83291bf9fd2 434
WD40andTape 3:c83291bf9fd2 435 pinGate0.rise(&rise0);
dofydoink 5:712e7634c779 436
WD40andTape 3:c83291bf9fd2 437 pinGate1.rise(&rise1);
WD40andTape 3:c83291bf9fd2 438 pinGate2.rise(&rise2);
WD40andTape 3:c83291bf9fd2 439 pinGate3.rise(&rise3);
WD40andTape 3:c83291bf9fd2 440 pinGate4.rise(&rise4);
WD40andTape 3:c83291bf9fd2 441 pinGate5.rise(&rise5);
WD40andTape 3:c83291bf9fd2 442 pinGate6.rise(&rise6);
WD40andTape 3:c83291bf9fd2 443 pinGate7.rise(&rise7);
dofydoink 5:712e7634c779 444
dofydoink 0:607bc887b6e0 445
dofydoink 0:607bc887b6e0 446 //set up fall interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 3:c83291bf9fd2 447 //
WD40andTape 3:c83291bf9fd2 448 //pinGate[0].fall(&testFallFunction);
WD40andTape 3:c83291bf9fd2 449 //testPinGate.fall(&testFallFunction); <working
WD40andTape 3:c83291bf9fd2 450 //testPinGate.fall(&fall0);
WD40andTape 3:c83291bf9fd2 451
WD40andTape 3:c83291bf9fd2 452 pinGate0.fall(&fall0);
dofydoink 5:712e7634c779 453
WD40andTape 3:c83291bf9fd2 454 pinGate1.fall(&fall1);
WD40andTape 3:c83291bf9fd2 455 pinGate2.fall(&fall2);
WD40andTape 3:c83291bf9fd2 456 pinGate3.fall(&fall3);
WD40andTape 3:c83291bf9fd2 457 pinGate4.fall(&fall4);
WD40andTape 3:c83291bf9fd2 458 pinGate5.fall(&fall5);
WD40andTape 3:c83291bf9fd2 459 pinGate6.fall(&fall6);
WD40andTape 3:c83291bf9fd2 460 pinGate7.fall(&fall7);
dofydoink 5:712e7634c779 461
dofydoink 0:607bc887b6e0 462
dofydoink 0:607bc887b6e0 463 timer.start();
dofydoink 0:607bc887b6e0 464
WD40andTape 4:303584310071 465 //threadSimulateDemand.start(SimulateDemand);
WD40andTape 4:303584310071 466 threadReceiveAndReplan.start(ReceiveAndReplan);//start Replanning thread
WD40andTape 4:303584310071 467 //ReceiveAndReplan();
WD40andTape 4:303584310071 468 threadSmoothPathPlan.start(CalculateSmoothPath); //start planning thread
WD40andTape 1:2a43cf183a62 469
dofydoink 0:607bc887b6e0 470 PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); //set up planning thread to recur at fixed intervals
WD40andTape 1:2a43cf183a62 471
dofydoink 0:607bc887b6e0 472 Thread::wait(1);
WD40andTape 3:c83291bf9fd2 473
dofydoink 0:607bc887b6e0 474
WD40andTape 1:2a43cf183a62 475 while(1) {
WD40andTape 1:2a43cf183a62 476 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 477 }
dofydoink 0:607bc887b6e0 478 }
dofydoink 0:607bc887b6e0 479