MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

main.cpp@0:607bc887b6e0, 2018-07-25 (annotated)

Committer:
dofydoink
Date:
Wed Jul 25 10:43:23 2018 +0000
Revision:
0:607bc887b6e0
Child:
1:2a43cf183a62
midlevel 4-1

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
dofydoink 0:607bc887b6e0 16 #define ADC_PRESSURE 1
dofydoink 0:607bc887b6e0 17 #define ADC_POSITION 3
dofydoink 0:607bc887b6e0 18
dofydoink 0:607bc887b6e0 19 #define N_CHANNELS 8 //number of channels to control
dofydoink 0:607bc887b6e0 20 //1-3: front segment
dofydoink 0:607bc887b6e0 21 //4-6: rear segment
dofydoink 0:607bc887b6e0 22 //7-8: mid segment
dofydoink 0:607bc887b6e0 23
dofydoink 0:607bc887b6e0 24 //parameters to manually change
dofydoink 0:607bc887b6e0 25 #define LOW_LEVEL_SPI_FREQUENCY 10000000
dofydoink 0:607bc887b6e0 26
dofydoink 0:607bc887b6e0 27 #define PATH_SAMPLE_TIME_S 0.05
dofydoink 0:607bc887b6e0 28
dofydoink 0:607bc887b6e0 29 #define MAX_LENGTH_MM 100.0
dofydoink 0:607bc887b6e0 30 #define MAX_ACTUATOR_LENGTH 50.0
dofydoink 0:607bc887b6e0 31 #define MAX_SPEED_MMPS 24.3457
dofydoink 0:607bc887b6e0 32 #define PATH_TOLERANCE_MM 0.2
dofydoink 0:607bc887b6e0 33
dofydoink 0:607bc887b6e0 34 double dblMaxChamberLengths_mm[N_CHANNELS] = {100.0,50.0,50.0,50.0,50.0,50.0,30.0,30.0};
dofydoink 0:607bc887b6e0 35
dofydoink 0:607bc887b6e0 36
dofydoink 0:607bc887b6e0 37
dofydoink 0:607bc887b6e0 38 int ii,jj,kk; //counting varaibles
dofydoink 0:607bc887b6e0 39
dofydoink 0:607bc887b6e0 40 //-----These are the variables being shared between High-Level and Mid-Level-----------------------------------------------//
dofydoink 0:607bc887b6e0 41
dofydoink 0:607bc887b6e0 42 double dblPSI[3][3]; //the message from high-level containing the chamber lengths in meters in following format:
dofydoink 0:607bc887b6e0 43 /*
dofydoink 0:607bc887b6e0 44 {L(front,1), L(front,2), L(front,3);
dofydoink 0:607bc887b6e0 45 L(mid,1) , L(mid,2) , L(mid,3);
dofydoink 0:607bc887b6e0 46 L(rear,1) , L(rear,2) , L(rear,3);}
dofydoink 0:607bc887b6e0 47 */
dofydoink 0:607bc887b6e0 48
dofydoink 0:607bc887b6e0 49 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 0:607bc887b6e0 50 double dblPressure_bar[N_CHANNELS]; //the pressure in a given chamber in bar (1 bar = 100,000 Pa).
dofydoink 0:607bc887b6e0 51 double dblTargetTime_s; //the time in seconds desired to reach the target (>0...!)
dofydoink 0:607bc887b6e0 52
dofydoink 0:607bc887b6e0 53 //
dofydoink 0:607bc887b6e0 54 Semaphore semReplan(1);// this must be set every time new high-level transmissions are received to allow replanning to take place!
dofydoink 0:607bc887b6e0 55 //--------------------------------------------------------------------------------------------------------------------------//
dofydoink 0:607bc887b6e0 56
dofydoink 0:607bc887b6e0 57 //boolean flag to indicate that new target information has been received
dofydoink 0:607bc887b6e0 58 //bool blnReplan;//set this when new transmission is received over ethernet
dofydoink 0:607bc887b6e0 59
dofydoink 0:607bc887b6e0 60 //path variables
dofydoink 0:607bc887b6e0 61 double dblVelocity_mmps[N_CHANNELS];//the linear path velocity (not sent to actuator)
dofydoink 0:607bc887b6e0 62 double dblLinearPath_mm[N_CHANNELS]; //the current position of the linear path (not sent to actuator)
dofydoink 0:607bc887b6e0 63 double dblSmoothPath_mm[N_CHANNELS]; //the current position of the smooth path (not sent to actuator)
dofydoink 0:607bc887b6e0 64 double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
dofydoink 0:607bc887b6e0 65 double dblTargetActLen_mm[N_CHANNELS];
dofydoink 0:607bc887b6e0 66 double dblPathToActuator[N_CHANNELS];//the target position for the actuator (sent to actuator)
dofydoink 0:607bc887b6e0 67
dofydoink 0:607bc887b6e0 68 int intDemPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator
dofydoink 0:607bc887b6e0 69 int intPosSPI_Rx[N_CHANNELS]; //13 bit value received over SPI from the actuator
dofydoink 0:607bc887b6e0 70 int intPosADC_Rx[N_CHANNELS]; //12-bit ADC reading of potentiometer on actuator
dofydoink 0:607bc887b6e0 71
dofydoink 0:607bc887b6e0 72 double dblPathTolerance; //how close the linear path must get to the target position before considering it a success.
dofydoink 0:607bc887b6e0 73
dofydoink 0:607bc887b6e0 74 double dblActuatorConversion[N_CHANNELS] = {0.24176,0.24176,0.24176,0.24176,0.24176,0.36264,0.36264};
dofydoink 0:607bc887b6e0 75
dofydoink 0:607bc887b6e0 76 double dblSmoothingFactor = 0.5;
dofydoink 0:607bc887b6e0 77
dofydoink 0:607bc887b6e0 78 char chrErrorFlag[N_CHANNELS];
dofydoink 0:607bc887b6e0 79
dofydoink 0:607bc887b6e0 80 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
dofydoink 0:607bc887b6e0 81
dofydoink 0:607bc887b6e0 82 SPI spi(PC_12,PC_11, PC_10); // mosi, miso, sclk
dofydoink 0:607bc887b6e0 83
dofydoink 0:607bc887b6e0 84 DigitalOut cs_LL[N_CHANNELS] = {PF_10, PD_13, PE_7, PD_12, PD_14, PD_11, PD_15, PE_10};//chip select for low level controller
dofydoink 0:607bc887b6e0 85 DigitalOut cs_ADC[N_CHANNELS] = {PF_1, PF_0, PD_1, PD_0, PG_0, PE_1, PG_9, PG_12}; //chip select for ADC
dofydoink 0:607bc887b6e0 86
dofydoink 0:607bc887b6e0 87 InterruptIn pinGate[N_CHANNELS] ={PE_11, PE_13, PF_3, PF_13, PF_15, PF_12, PF_11, PG_14};//gate interrupt pins
dofydoink 0:607bc887b6e0 88
dofydoink 0:607bc887b6e0 89 DigitalOut pinReset(PD_2); //reset pin for all controllers.
dofydoink 0:607bc887b6e0 90
dofydoink 0:607bc887b6e0 91 EventQueue queue(32 * EVENTS_EVENT_SIZE);
dofydoink 0:607bc887b6e0 92
dofydoink 0:607bc887b6e0 93 Thread t(osPriorityRealtime);
dofydoink 0:607bc887b6e0 94
dofydoink 0:607bc887b6e0 95 Thread threadReplan(osPriorityBelowNormal);
dofydoink 0:607bc887b6e0 96 Thread threadPathPlan(osPriorityNormal);
dofydoink 0:607bc887b6e0 97
dofydoink 0:607bc887b6e0 98 Thread threadSimulateDemand(osPriorityHigh);
dofydoink 0:607bc887b6e0 99
dofydoink 0:607bc887b6e0 100 Mutex mut[N_CHANNELS];
dofydoink 0:607bc887b6e0 101
dofydoink 0:607bc887b6e0 102 Semaphore semPathPlan(1);//
dofydoink 0:607bc887b6e0 103
dofydoink 0:607bc887b6e0 104 Timer timer;//timers are nice
dofydoink 0:607bc887b6e0 105
dofydoink 0:607bc887b6e0 106
dofydoink 0:607bc887b6e0 107 /*
dofydoink 0:607bc887b6e0 108 unsigned int result1 = 0;
dofydoink 0:607bc887b6e0 109 unsigned int result2 = 0;
dofydoink 0:607bc887b6e0 110 unsigned int result3 = 0;
dofydoink 0:607bc887b6e0 111 unsigned int result4 = 0;
dofydoink 0:607bc887b6e0 112
dofydoink 0:607bc887b6e0 113 unsigned int result1a = 0;
dofydoink 0:607bc887b6e0 114 unsigned int result1b = 0;
dofydoink 0:607bc887b6e0 115 */
dofydoink 0:607bc887b6e0 116
dofydoink 0:607bc887b6e0 117
dofydoink 0:607bc887b6e0 118
dofydoink 0:607bc887b6e0 119
dofydoink 0:607bc887b6e0 120 double dblDemPosition[N_CHANNELS];
dofydoink 0:607bc887b6e0 121 double dblPosition[N_CHANNELS];
dofydoink 0:607bc887b6e0 122 double dblPressure[N_CHANNELS];
dofydoink 0:607bc887b6e0 123
dofydoink 0:607bc887b6e0 124 int ThreadID[N_CHANNELS];
dofydoink 0:607bc887b6e0 125
dofydoink 0:607bc887b6e0 126 bool blnDataReady[N_CHANNELS];
dofydoink 0:607bc887b6e0 127
dofydoink 0:607bc887b6e0 128 unsigned int ReadADCPosition(int channel)
dofydoink 0:607bc887b6e0 129 {
dofydoink 0:607bc887b6e0 130 unsigned int outputA;
dofydoink 0:607bc887b6e0 131 unsigned int outputB;
dofydoink 0:607bc887b6e0 132 unsigned int output;
dofydoink 0:607bc887b6e0 133
dofydoink 0:607bc887b6e0 134 spi.format(8,0);
dofydoink 0:607bc887b6e0 135 spi.frequency(1000000);
dofydoink 0:607bc887b6e0 136
dofydoink 0:607bc887b6e0 137 cs_ADC[channel] = 0;
dofydoink 0:607bc887b6e0 138 spi.write(PREAMBLE);
dofydoink 0:607bc887b6e0 139 outputA = spi.write(CHAN_3);
dofydoink 0:607bc887b6e0 140 outputB = spi.write(0xFF);
dofydoink 0:607bc887b6e0 141 cs_ADC[channel] = 1;
dofydoink 0:607bc887b6e0 142
dofydoink 0:607bc887b6e0 143 outputA = outputA & DATA_MASK;
dofydoink 0:607bc887b6e0 144 outputA = outputA<<8;
dofydoink 0:607bc887b6e0 145 output = (outputA | outputB);
dofydoink 0:607bc887b6e0 146
dofydoink 0:607bc887b6e0 147 return output;
dofydoink 0:607bc887b6e0 148 }
dofydoink 0:607bc887b6e0 149
dofydoink 0:607bc887b6e0 150 unsigned int ReadADCPressure(int channel)
dofydoink 0:607bc887b6e0 151 {
dofydoink 0:607bc887b6e0 152 unsigned int outputA;
dofydoink 0:607bc887b6e0 153 unsigned int outputB;
dofydoink 0:607bc887b6e0 154 unsigned int output;
dofydoink 0:607bc887b6e0 155
dofydoink 0:607bc887b6e0 156 spi.format(8,0);
dofydoink 0:607bc887b6e0 157 spi.frequency(1000000);
dofydoink 0:607bc887b6e0 158
dofydoink 0:607bc887b6e0 159 cs_ADC[channel] = 0;
dofydoink 0:607bc887b6e0 160 spi.write(PREAMBLE);
dofydoink 0:607bc887b6e0 161 outputA = spi.write(CHAN_1);
dofydoink 0:607bc887b6e0 162 outputB = spi.write(0xFF);
dofydoink 0:607bc887b6e0 163 cs_ADC[channel] = 1;
dofydoink 0:607bc887b6e0 164
dofydoink 0:607bc887b6e0 165 outputA = outputA & DATA_MASK;
dofydoink 0:607bc887b6e0 166 outputA = outputA<<8;
dofydoink 0:607bc887b6e0 167 output = (outputA | outputB);
dofydoink 0:607bc887b6e0 168
dofydoink 0:607bc887b6e0 169 return output;
dofydoink 0:607bc887b6e0 170 }
dofydoink 0:607bc887b6e0 171
dofydoink 0:607bc887b6e0 172 void TransmitData(int channel)
dofydoink 0:607bc887b6e0 173 {
dofydoink 0:607bc887b6e0 174 //get data from controller
dofydoink 0:607bc887b6e0 175 spi.format(16,2);
dofydoink 0:607bc887b6e0 176 spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
dofydoink 0:607bc887b6e0 177
dofydoink 0:607bc887b6e0 178 cs_LL[channel] = 0;//select relevant chip
dofydoink 0:607bc887b6e0 179 intPosSPI_Rx[channel] = spi.write(intDemPos_Tx[channel]); //transmit & receive
dofydoink 0:607bc887b6e0 180 cs_LL[channel] = 1;//deselect chip
dofydoink 0:607bc887b6e0 181
dofydoink 0:607bc887b6e0 182 //sort out received data
dofydoink 0:607bc887b6e0 183 chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
dofydoink 0:607bc887b6e0 184 intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
dofydoink 0:607bc887b6e0 185 dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*MAX_ACTUATOR_LENGTH/dblActuatorConversion[channel];
dofydoink 0:607bc887b6e0 186 }
dofydoink 0:607bc887b6e0 187
dofydoink 0:607bc887b6e0 188 //common rise handler function
dofydoink 0:607bc887b6e0 189
dofydoink 0:607bc887b6e0 190 void common_rise_handler(int channel)
dofydoink 0:607bc887b6e0 191 {
dofydoink 0:607bc887b6e0 192 //check if data is ready for tranmission
dofydoink 0:607bc887b6e0 193 if (blnDataReady[channel])
dofydoink 0:607bc887b6e0 194 {
dofydoink 0:607bc887b6e0 195 // Add transmit task to event queue
dofydoink 0:607bc887b6e0 196 blnDataReady[channel] = 0;//data no longer ready
dofydoink 0:607bc887b6e0 197 ThreadID[channel] = queue.call(TransmitData,channel);//schedule transmission
dofydoink 0:607bc887b6e0 198 }
dofydoink 0:607bc887b6e0 199 }
dofydoink 0:607bc887b6e0 200
dofydoink 0:607bc887b6e0 201
dofydoink 0:607bc887b6e0 202
dofydoink 0:607bc887b6e0 203 //common_fall handler functions
dofydoink 0:607bc887b6e0 204 void common_fall_handler(int channel)
dofydoink 0:607bc887b6e0 205 {
dofydoink 0:607bc887b6e0 206 //cancel relevant queued event
dofydoink 0:607bc887b6e0 207 queue.cancel(ThreadID[channel]);
dofydoink 0:607bc887b6e0 208 }
dofydoink 0:607bc887b6e0 209
dofydoink 0:607bc887b6e0 210 //stub rise functions
dofydoink 0:607bc887b6e0 211 void rise0(void) { common_rise_handler(0); }
dofydoink 0:607bc887b6e0 212 void rise1(void) { common_rise_handler(1); }
dofydoink 0:607bc887b6e0 213 void rise2(void) { common_rise_handler(2); }
dofydoink 0:607bc887b6e0 214 void rise3(void) { common_rise_handler(3); }
dofydoink 0:607bc887b6e0 215 void rise4(void) { common_rise_handler(4); }
dofydoink 0:607bc887b6e0 216 void rise5(void) { common_rise_handler(5); }
dofydoink 0:607bc887b6e0 217 void rise6(void) { common_rise_handler(6); }
dofydoink 0:607bc887b6e0 218 void rise7(void) { common_rise_handler(7); }
dofydoink 0:607bc887b6e0 219
dofydoink 0:607bc887b6e0 220 //stub fall functions
dofydoink 0:607bc887b6e0 221 void fall0(void) { common_fall_handler(0); }
dofydoink 0:607bc887b6e0 222 void fall1(void) { common_fall_handler(1); }
dofydoink 0:607bc887b6e0 223 void fall2(void) { common_fall_handler(2); }
dofydoink 0:607bc887b6e0 224 void fall3(void) { common_fall_handler(3); }
dofydoink 0:607bc887b6e0 225 void fall4(void) { common_fall_handler(4); }
dofydoink 0:607bc887b6e0 226 void fall5(void) { common_fall_handler(5); }
dofydoink 0:607bc887b6e0 227 void fall6(void) { common_fall_handler(6); }
dofydoink 0:607bc887b6e0 228 void fall7(void) { common_fall_handler(7); }
dofydoink 0:607bc887b6e0 229
dofydoink 0:607bc887b6e0 230 void startPathPlan()
dofydoink 0:607bc887b6e0 231 {
dofydoink 0:607bc887b6e0 232 semPathPlan.release();
dofydoink 0:607bc887b6e0 233 }
dofydoink 0:607bc887b6e0 234
dofydoink 0:607bc887b6e0 235 //this function will be called when a new transmission is received from high level
dofydoink 0:607bc887b6e0 236 void ReplanPath()
dofydoink 0:607bc887b6e0 237 {
dofydoink 0:607bc887b6e0 238 //while(1)
dofydoink 0:607bc887b6e0 239 //{
dofydoink 0:607bc887b6e0 240 //semReplan.wait();//wait until called
dofydoink 0:607bc887b6e0 241 //printf("replan!\r\n");
dofydoink 0:607bc887b6e0 242 for (ii = 0; ii < N_CHANNELS; ii++)
dofydoink 0:607bc887b6e0 243 {
dofydoink 0:607bc887b6e0 244 mut[ii].lock();
dofydoink 0:607bc887b6e0 245 }
dofydoink 0:607bc887b6e0 246
dofydoink 0:607bc887b6e0 247 //update front segment
dofydoink 0:607bc887b6e0 248 dblTargetChambLen_mm[0] = dblPSI[0][0]*1000;
dofydoink 0:607bc887b6e0 249 dblTargetChambLen_mm[1] = dblPSI[0][1]*1000;
dofydoink 0:607bc887b6e0 250 dblTargetChambLen_mm[2] = dblPSI[0][2]*1000;
dofydoink 0:607bc887b6e0 251 //update mid segment
dofydoink 0:607bc887b6e0 252 dblTargetChambLen_mm[6] = dblPSI[1][0]*1000;
dofydoink 0:607bc887b6e0 253 dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; //same because two pumps are used
dofydoink 0:607bc887b6e0 254 //update rear segment
dofydoink 0:607bc887b6e0 255 dblTargetChambLen_mm[3] = dblPSI[2][0]*1000;
dofydoink 0:607bc887b6e0 256 dblTargetChambLen_mm[4] = dblPSI[2][1]*1000;
dofydoink 0:607bc887b6e0 257 dblTargetChambLen_mm[5] = dblPSI[2][2]*1000;
dofydoink 0:607bc887b6e0 258
dofydoink 0:607bc887b6e0 259 for (ii = 0; ii < N_CHANNELS; ii++)
dofydoink 0:607bc887b6e0 260 {
dofydoink 0:607bc887b6e0 261 mut[ii].unlock();
dofydoink 0:607bc887b6e0 262 }
dofydoink 0:607bc887b6e0 263
dofydoink 0:607bc887b6e0 264 for (ii = 0; ii< N_CHANNELS; ii++)
dofydoink 0:607bc887b6e0 265 {
dofydoink 0:607bc887b6e0 266 mut[ii].lock();//lock variables to avoid race condition
dofydoink 0:607bc887b6e0 267
dofydoink 0:607bc887b6e0 268 //check to see if positions are achievable
dofydoink 0:607bc887b6e0 269 if(dblTargetChambLen_mm[ii]>dblMaxChamberLengths_mm[ii])
dofydoink 0:607bc887b6e0 270 {
dofydoink 0:607bc887b6e0 271 dblTargetChambLen_mm[ii] = dblMaxChamberLengths_mm[ii];
dofydoink 0:607bc887b6e0 272 }
dofydoink 0:607bc887b6e0 273
dofydoink 0:607bc887b6e0 274 if(dblTargetChambLen_mm[ii]<0.0)
dofydoink 0:607bc887b6e0 275 {
dofydoink 0:607bc887b6e0 276 dblTargetChambLen_mm[ii] = 0.0;
dofydoink 0:607bc887b6e0 277 }
dofydoink 0:607bc887b6e0 278
dofydoink 0:607bc887b6e0 279 dblTargetActLen_mm[ii] = dblTargetChambLen_mm[ii]*dblActuatorConversion[ii];
dofydoink 0:607bc887b6e0 280
dofydoink 0:607bc887b6e0 281 //work out new velocities
dofydoink 0:607bc887b6e0 282 dblVelocity_mmps[ii] = (dblTargetActLen_mm[ii] - dblLinearPath_mm[ii]) / dblTargetTime_s;
dofydoink 0:607bc887b6e0 283
dofydoink 0:607bc887b6e0 284 //check to see if velocities are achievable
dofydoink 0:607bc887b6e0 285 if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS)
dofydoink 0:607bc887b6e0 286 {
dofydoink 0:607bc887b6e0 287 if(dblVelocity_mmps[ii]>0)
dofydoink 0:607bc887b6e0 288 {
dofydoink 0:607bc887b6e0 289 dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
dofydoink 0:607bc887b6e0 290 }
dofydoink 0:607bc887b6e0 291 else
dofydoink 0:607bc887b6e0 292 {
dofydoink 0:607bc887b6e0 293 dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
dofydoink 0:607bc887b6e0 294 }
dofydoink 0:607bc887b6e0 295 }
dofydoink 0:607bc887b6e0 296
dofydoink 0:607bc887b6e0 297 mut[ii].unlock(); //unlock mutex.
dofydoink 0:607bc887b6e0 298 }
dofydoink 0:607bc887b6e0 299 //}
dofydoink 0:607bc887b6e0 300
dofydoink 0:607bc887b6e0 301
dofydoink 0:607bc887b6e0 302 }
dofydoink 0:607bc887b6e0 303
dofydoink 0:607bc887b6e0 304 void CalculatePath()
dofydoink 0:607bc887b6e0 305 {
dofydoink 0:607bc887b6e0 306 while(1)
dofydoink 0:607bc887b6e0 307 {
dofydoink 0:607bc887b6e0 308 semPathPlan.wait();
dofydoink 0:607bc887b6e0 309 //if(blnReplan)
dofydoink 0:607bc887b6e0 310 // {
dofydoink 0:607bc887b6e0 311 // blnReplan = 0;//remove flag
dofydoink 0:607bc887b6e0 312 // ReplanPath(dblPSI, dblTargetTime_s);
dofydoink 0:607bc887b6e0 313 // }
dofydoink 0:607bc887b6e0 314 for(ii = 0; ii < N_CHANNELS; ii++)
dofydoink 0:607bc887b6e0 315 {
dofydoink 0:607bc887b6e0 316 //calculate next step in linear path
dofydoink 0:607bc887b6e0 317 mut[ii].lock();//lock relevant mutex
dofydoink 0:607bc887b6e0 318 dblLinearPath_mm[ii] = dblLinearPath_mm[ii] + dblVelocity_mmps[ii]*PATH_SAMPLE_TIME_S;
dofydoink 0:607bc887b6e0 319 //check tolerance
dofydoink 0:607bc887b6e0 320 if (abs(dblLinearPath_mm[ii] - dblTargetActLen_mm[ii]) <= dblPathTolerance)
dofydoink 0:607bc887b6e0 321 {
dofydoink 0:607bc887b6e0 322 dblVelocity_mmps[ii] = 0; //stop linear path generation when linear path is within tolerance of target position.
dofydoink 0:607bc887b6e0 323 }
dofydoink 0:607bc887b6e0 324 mut[ii].unlock();//unlock relevant mutex
dofydoink 0:607bc887b6e0 325
dofydoink 0:607bc887b6e0 326 //calculate next step in smooth path
dofydoink 0:607bc887b6e0 327 dblSmoothPath_mm[ii] = dblSmoothingFactor*dblLinearPath_mm[ii] + (1.0-dblSmoothingFactor)*dblSmoothPath_mm[ii];
dofydoink 0:607bc887b6e0 328
dofydoink 0:607bc887b6e0 329 //convert to actuator distances
dofydoink 0:607bc887b6e0 330 dblPathToActuator[ii] = dblSmoothPath_mm[ii];
dofydoink 0:607bc887b6e0 331
dofydoink 0:607bc887b6e0 332 intDemPos_Tx[ii] = (int) dblPathToActuator[ii]/MAX_ACTUATOR_LENGTH*8191;//convert to a 13-bit number;
dofydoink 0:607bc887b6e0 333 intDemPos_Tx[ii] = intDemPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit
dofydoink 0:607bc887b6e0 334
dofydoink 0:607bc887b6e0 335
dofydoink 0:607bc887b6e0 336 blnDataReady[ii] = 1;//signal that data ready
dofydoink 0:607bc887b6e0 337
dofydoink 0:607bc887b6e0 338 }
dofydoink 0:607bc887b6e0 339 printf("%d, %f,%f,%f, %f\r\n",timer.read_ms(), dblTargetActLen_mm[0] ,dblVelocity_mmps[0], dblLinearPath_mm[0], dblSmoothPath_mm[0]);
dofydoink 0:607bc887b6e0 340 }
dofydoink 0:607bc887b6e0 341 }
dofydoink 0:607bc887b6e0 342
dofydoink 0:607bc887b6e0 343 void SimulateDemand()
dofydoink 0:607bc887b6e0 344 {
dofydoink 0:607bc887b6e0 345 while(1)
dofydoink 0:607bc887b6e0 346 {
dofydoink 0:607bc887b6e0 347 mut[0].lock();
dofydoink 0:607bc887b6e0 348 if(kk == 0)
dofydoink 0:607bc887b6e0 349 {
dofydoink 0:607bc887b6e0 350 dblPSI[0][0] = (double) 10.0;
dofydoink 0:607bc887b6e0 351 dblTargetTime_s = 1.0;
dofydoink 0:607bc887b6e0 352 }
dofydoink 0:607bc887b6e0 353 else
dofydoink 0:607bc887b6e0 354 {
dofydoink 0:607bc887b6e0 355 dblPSI[0][0] = (double) 0.0;
dofydoink 0:607bc887b6e0 356 dblTargetTime_s = 2.0;
dofydoink 0:607bc887b6e0 357 }
dofydoink 0:607bc887b6e0 358
dofydoink 0:607bc887b6e0 359 kk = 1 - kk;
dofydoink 0:607bc887b6e0 360
dofydoink 0:607bc887b6e0 361
dofydoink 0:607bc887b6e0 362 //semReplan.release();
dofydoink 0:607bc887b6e0 363
dofydoink 0:607bc887b6e0 364 mut[0].unlock();
dofydoink 0:607bc887b6e0 365 ReplanPath();
dofydoink 0:607bc887b6e0 366
dofydoink 0:607bc887b6e0 367 Thread::wait(7000);
dofydoink 0:607bc887b6e0 368 }
dofydoink 0:607bc887b6e0 369 }
dofydoink 0:607bc887b6e0 370
dofydoink 0:607bc887b6e0 371
dofydoink 0:607bc887b6e0 372
dofydoink 0:607bc887b6e0 373 Ticker PathCalculationTicker;
dofydoink 0:607bc887b6e0 374
dofydoink 0:607bc887b6e0 375 int main()
dofydoink 0:607bc887b6e0 376 {
dofydoink 0:607bc887b6e0 377 //initialise relevant variables
dofydoink 0:607bc887b6e0 378 for(ii = 0; ii<N_CHANNELS; ii++)
dofydoink 0:607bc887b6e0 379 {
dofydoink 0:607bc887b6e0 380 //all chip selects in off state
dofydoink 0:607bc887b6e0 381 cs_LL[ii] = 1;
dofydoink 0:607bc887b6e0 382 cs_ADC[ii] = 1;
dofydoink 0:607bc887b6e0 383
dofydoink 0:607bc887b6e0 384 //data ready flags set to not ready
dofydoink 0:607bc887b6e0 385 blnDataReady[ii] = 0;
dofydoink 0:607bc887b6e0 386 }
dofydoink 0:607bc887b6e0 387
dofydoink 0:607bc887b6e0 388 //calculate some control variables
dofydoink 0:607bc887b6e0 389 dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance.
dofydoink 0:607bc887b6e0 390
dofydoink 0:607bc887b6e0 391 pinReset = 1; //initialise reset pin to not reset the controllers.
dofydoink 0:607bc887b6e0 392
dofydoink 0:607bc887b6e0 393 //say something nice to the user.
dofydoink 0:607bc887b6e0 394 pc.baud(9600);
dofydoink 0:607bc887b6e0 395 printf("Hi, there! I'll be your mid-level controller for today.\r\n");
dofydoink 0:607bc887b6e0 396
dofydoink 0:607bc887b6e0 397 // Start the event queue
dofydoink 0:607bc887b6e0 398 t.start(callback(&queue, &EventQueue::dispatch_forever));
dofydoink 0:607bc887b6e0 399
dofydoink 0:607bc887b6e0 400 //set up the interrupts
dofydoink 0:607bc887b6e0 401
dofydoink 0:607bc887b6e0 402 //set up rise interrupts MIGHT NOT NEED TO BE POINTERS
dofydoink 0:607bc887b6e0 403 pinGate[0].rise(&rise0);
dofydoink 0:607bc887b6e0 404 pinGate[1].rise(&rise1);
dofydoink 0:607bc887b6e0 405 pinGate[2].rise(&rise2);
dofydoink 0:607bc887b6e0 406 pinGate[3].rise(&rise3);
dofydoink 0:607bc887b6e0 407 pinGate[4].rise(&rise4);
dofydoink 0:607bc887b6e0 408 pinGate[5].rise(&rise5);
dofydoink 0:607bc887b6e0 409 pinGate[6].rise(&rise6);
dofydoink 0:607bc887b6e0 410 pinGate[7].rise(&rise7);
dofydoink 0:607bc887b6e0 411
dofydoink 0:607bc887b6e0 412 //set up fall interrupts MIGHT NOT NEED TO BE POINTERS
dofydoink 0:607bc887b6e0 413 pinGate[0].fall(&fall0);
dofydoink 0:607bc887b6e0 414 pinGate[1].fall(&fall1);
dofydoink 0:607bc887b6e0 415 pinGate[2].fall(&fall2);
dofydoink 0:607bc887b6e0 416 pinGate[3].fall(&fall3);
dofydoink 0:607bc887b6e0 417 pinGate[4].fall(&fall4);
dofydoink 0:607bc887b6e0 418 pinGate[5].fall(&fall5);
dofydoink 0:607bc887b6e0 419 pinGate[6].fall(&fall6);
dofydoink 0:607bc887b6e0 420 pinGate[7].fall(&fall7);
dofydoink 0:607bc887b6e0 421
dofydoink 0:607bc887b6e0 422 timer.start();
dofydoink 0:607bc887b6e0 423 kk = 0;
dofydoink 0:607bc887b6e0 424
dofydoink 0:607bc887b6e0 425 threadSimulateDemand.start(SimulateDemand);
dofydoink 0:607bc887b6e0 426 threadPathPlan.start(CalculatePath); //start planning thread
dofydoink 0:607bc887b6e0 427 PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); //set up planning thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 428 threadReplan.start(ReplanPath);//start Replanning thread
dofydoink 0:607bc887b6e0 429 Thread::wait(1);
dofydoink 0:607bc887b6e0 430
dofydoink 0:607bc887b6e0 431
dofydoink 0:607bc887b6e0 432 while(1) {
dofydoink 0:607bc887b6e0 433
dofydoink 0:607bc887b6e0 434
dofydoink 0:607bc887b6e0 435
dofydoink 0:607bc887b6e0 436 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 437 }
dofydoink 0:607bc887b6e0 438 }
dofydoink 0:607bc887b6e0 439