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@1:2a43cf183a62, 2018-07-25 (annotated)

Committer:
WD40andTape
Date:
Wed Jul 25 13:43:59 2018 +0000
Revision:
1:2a43cf183a62
Parent:
0:607bc887b6e0
Child:
2:eea12b149dba
Improved variable names and fixed some real-time issues.

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