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@3:c83291bf9fd2, 2018-07-31 (annotated)

Committer:
WD40andTape
Date:
Tue Jul 31 14:30:40 2018 +0000
Revision:
3:c83291bf9fd2
Parent:
2:eea12b149dba
Child:
4:303584310071
Bug fixes. Tested and working without communication.

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
WD40andTape 3:c83291bf9fd2 32 #define MAX_ACTUATOR_LENGTH 52.2
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
WD40andTape 2:eea12b149dba 40 int ii,jj; //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)
WD40andTape 2:eea12b149dba 64 double dblLinearPathCurrentPos_mm[N_CHANNELS]; //the current position of the linear path (not sent to actuator)
WD40andTape 2:eea12b149dba 65 double dblSmoothPathCurrentPos_mm[N_CHANNELS]; //the current position of the smooth path (not sent to actuator)
WD40andTape 2:eea12b149dba 66
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
WD40andTape 3:c83291bf9fd2 84
WD40andTape 3:c83291bf9fd2 85 InterruptIn pinGate6(PE_11); //this pin HAS TO BE defined before SPI set up. No Clue Why.
WD40andTape 3:c83291bf9fd2 86
WD40andTape 3:c83291bf9fd2 87 SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
WD40andTape 3:c83291bf9fd2 88
WD40andTape 3:c83291bf9fd2 89 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 90 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 91
WD40andTape 3:c83291bf9fd2 92 DigitalOut pinCheck(PE_5);
WD40andTape 3:c83291bf9fd2 93
WD40andTape 3:c83291bf9fd2 94 //InterruptIn pinGate[N_CHANNELS] ={PF_11, PG_14, PF_15, PF_12, PF_3, PF_13, PE_11, PE_13};//gate interrupt pins
dofydoink 0:607bc887b6e0 95
WD40andTape 3:c83291bf9fd2 96 //WRONG!!!!!!!!!!
WD40andTape 3:c83291bf9fd2 97 //InterruptIn pinGate[N_CHANNELS] ={PG_11, PG_14, PF_15, PF_12, PF_3, PF_13, PE_11, PE_13};//gate interrupt pins
WD40andTape 3:c83291bf9fd2 98 //WRONG!!!!!
dofydoink 0:607bc887b6e0 99
WD40andTape 3:c83291bf9fd2 100 //InterruptIn testPinGate(PF_11);
WD40andTape 3:c83291bf9fd2 101 //these interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
WD40andTape 3:c83291bf9fd2 102 InterruptIn pinGate0(PF_11);
WD40andTape 3:c83291bf9fd2 103 InterruptIn pinGate1(PG_14);
WD40andTape 3:c83291bf9fd2 104 InterruptIn pinGate2(PF_15);
WD40andTape 3:c83291bf9fd2 105 InterruptIn pinGate3(PF_12);
WD40andTape 3:c83291bf9fd2 106 InterruptIn pinGate4(PF_3);
WD40andTape 3:c83291bf9fd2 107 InterruptIn pinGate5(PF_13);
WD40andTape 3:c83291bf9fd2 108 //InterruptIn pinGate6(PE_11); //
WD40andTape 3:c83291bf9fd2 109 InterruptIn pinGate7(PE_13);
dofydoink 0:607bc887b6e0 110
dofydoink 0:607bc887b6e0 111 DigitalOut pinReset(PD_2); //reset pin for all controllers.
dofydoink 0:607bc887b6e0 112
WD40andTape 3:c83291bf9fd2 113
dofydoink 0:607bc887b6e0 114 EventQueue queue(32 * EVENTS_EVENT_SIZE);
dofydoink 0:607bc887b6e0 115
dofydoink 0:607bc887b6e0 116 Thread t(osPriorityRealtime);
dofydoink 0:607bc887b6e0 117
dofydoink 0:607bc887b6e0 118 Thread threadReplan(osPriorityBelowNormal);
dofydoink 0:607bc887b6e0 119 Thread threadPathPlan(osPriorityNormal);
dofydoink 0:607bc887b6e0 120
dofydoink 0:607bc887b6e0 121 Thread threadSimulateDemand(osPriorityHigh);
dofydoink 0:607bc887b6e0 122
WD40andTape 1:2a43cf183a62 123 Mutex mutChannel[N_CHANNELS];
WD40andTape 1:2a43cf183a62 124 Mutex mutPsi;
dofydoink 0:607bc887b6e0 125
dofydoink 0:607bc887b6e0 126 Semaphore semPathPlan(1);//
dofydoink 0:607bc887b6e0 127
dofydoink 0:607bc887b6e0 128 Timer timer;//timers are nice
dofydoink 0:607bc887b6e0 129
dofydoink 0:607bc887b6e0 130
dofydoink 0:607bc887b6e0 131 /*
dofydoink 0:607bc887b6e0 132 unsigned int result1 = 0;
dofydoink 0:607bc887b6e0 133 unsigned int result2 = 0;
dofydoink 0:607bc887b6e0 134 unsigned int result3 = 0;
dofydoink 0:607bc887b6e0 135 unsigned int result4 = 0;
dofydoink 0:607bc887b6e0 136
dofydoink 0:607bc887b6e0 137 unsigned int result1a = 0;
dofydoink 0:607bc887b6e0 138 unsigned int result1b = 0;
dofydoink 0:607bc887b6e0 139 */
dofydoink 0:607bc887b6e0 140
dofydoink 0:607bc887b6e0 141
dofydoink 0:607bc887b6e0 142
WD40andTape 1:2a43cf183a62 143 /*
WD40andTape 1:2a43cf183a62 144 double dblDemandPosition[N_CHANNELS];
dofydoink 0:607bc887b6e0 145 double dblPosition[N_CHANNELS];
dofydoink 0:607bc887b6e0 146 double dblPressure[N_CHANNELS];
WD40andTape 1:2a43cf183a62 147 */
dofydoink 0:607bc887b6e0 148
dofydoink 0:607bc887b6e0 149 int ThreadID[N_CHANNELS];
dofydoink 0:607bc887b6e0 150
WD40andTape 1:2a43cf183a62 151 bool isDataReady[N_CHANNELS]; // flag to indicate path data is ready for transmission to low level.
dofydoink 0:607bc887b6e0 152
WD40andTape 3:c83291bf9fd2 153 double ReadADCPosition(int channel)
dofydoink 0:607bc887b6e0 154 {
dofydoink 0:607bc887b6e0 155 unsigned int outputA;
dofydoink 0:607bc887b6e0 156 unsigned int outputB;
WD40andTape 3:c83291bf9fd2 157 int output;
WD40andTape 3:c83291bf9fd2 158 double dblOutput;
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_3);
dofydoink 0:607bc887b6e0 166 outputB = spi.write(0xFF);
WD40andTape 1:2a43cf183a62 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);
WD40andTape 3:c83291bf9fd2 172 output = 4095- output;
WD40andTape 3:c83291bf9fd2 173 dblOutput = (double) (output - 1504)/4095*100.0;
WD40andTape 3:c83291bf9fd2 174 return dblOutput;
dofydoink 0:607bc887b6e0 175 }
dofydoink 0:607bc887b6e0 176
WD40andTape 3:c83291bf9fd2 177 double ReadADCPressure(int channel)
dofydoink 0:607bc887b6e0 178 {
dofydoink 0:607bc887b6e0 179 unsigned int outputA;
dofydoink 0:607bc887b6e0 180 unsigned int outputB;
WD40andTape 3:c83291bf9fd2 181 int output;
WD40andTape 3:c83291bf9fd2 182 double dblOutput;
dofydoink 0:607bc887b6e0 183
dofydoink 0:607bc887b6e0 184 spi.format(8,0);
dofydoink 0:607bc887b6e0 185 spi.frequency(1000000);
dofydoink 0:607bc887b6e0 186
dofydoink 0:607bc887b6e0 187 cs_ADC[channel] = 0;
dofydoink 0:607bc887b6e0 188 spi.write(PREAMBLE);
dofydoink 0:607bc887b6e0 189 outputA = spi.write(CHAN_1);
dofydoink 0:607bc887b6e0 190 outputB = spi.write(0xFF);
dofydoink 0:607bc887b6e0 191 cs_ADC[channel] = 1;
dofydoink 0:607bc887b6e0 192
dofydoink 0:607bc887b6e0 193 outputA = outputA & DATA_MASK;
dofydoink 0:607bc887b6e0 194 outputA = outputA<<8;
dofydoink 0:607bc887b6e0 195 output = (outputA | outputB);
dofydoink 0:607bc887b6e0 196
WD40andTape 3:c83291bf9fd2 197 dblOutput = (double) (output-502)/4095*8.0;
WD40andTape 3:c83291bf9fd2 198 return dblOutput;
dofydoink 0:607bc887b6e0 199 }
dofydoink 0:607bc887b6e0 200
WD40andTape 1:2a43cf183a62 201 void SendReceiveData(int channel)
dofydoink 0:607bc887b6e0 202 {
WD40andTape 3:c83291bf9fd2 203
dofydoink 0:607bc887b6e0 204 //get data from controller
dofydoink 0:607bc887b6e0 205 spi.format(16,2);
dofydoink 0:607bc887b6e0 206 spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
dofydoink 0:607bc887b6e0 207
dofydoink 0:607bc887b6e0 208 cs_LL[channel] = 0;//select relevant chip
WD40andTape 1:2a43cf183a62 209 intPosSPI_Rx[channel] = spi.write(intDemandPos_Tx[channel]); //transmit & receive
dofydoink 0:607bc887b6e0 210 cs_LL[channel] = 1;//deselect chip
WD40andTape 1:2a43cf183a62 211 isDataReady[channel] = 0;//data no longer ready, i.e. we now require new data
WD40andTape 1:2a43cf183a62 212
WD40andTape 3:c83291bf9fd2 213
dofydoink 0:607bc887b6e0 214
dofydoink 0:607bc887b6e0 215 //sort out received data
dofydoink 0:607bc887b6e0 216 chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
dofydoink 0:607bc887b6e0 217 intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
WD40andTape 1:2a43cf183a62 218 dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/dblActuatorConversion[channel])/1000;
WD40andTape 3:c83291bf9fd2 219 //printf("%d, %d\r\n",intPosSPI_Rx[0],intDemandPos_Tx[0]);
WD40andTape 3:c83291bf9fd2 220
WD40andTape 3:c83291bf9fd2 221 }
WD40andTape 3:c83291bf9fd2 222
WD40andTape 3:c83291bf9fd2 223 void testSendReceiveData()
WD40andTape 3:c83291bf9fd2 224 {
WD40andTape 3:c83291bf9fd2 225 printf("x\r\n");
WD40andTape 3:c83291bf9fd2 226 //get data from controller
WD40andTape 3:c83291bf9fd2 227 spi.format(16,2);
WD40andTape 3:c83291bf9fd2 228 spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 3:c83291bf9fd2 229
WD40andTape 3:c83291bf9fd2 230 cs_LL[0] = 0;//select relevant chip
WD40andTape 3:c83291bf9fd2 231 intPosSPI_Rx[0] = spi.write(intDemandPos_Tx[0]); //transmit & receive
WD40andTape 3:c83291bf9fd2 232 cs_LL[0] = 1;//deselect chip
WD40andTape 3:c83291bf9fd2 233 isDataReady[0] = 0;//data no longer ready, i.e. we now require new data
WD40andTape 3:c83291bf9fd2 234
WD40andTape 3:c83291bf9fd2 235 //sort out received data
WD40andTape 3:c83291bf9fd2 236 chrErrorFlag[0] = intPosSPI_Rx[0]>>13;
WD40andTape 3:c83291bf9fd2 237 intPosSPI_Rx[0] = intPosSPI_Rx[0] & 0x1FFF;
WD40andTape 3:c83291bf9fd2 238 dblPosition_mtrs[0] = (double)intPosSPI_Rx[0]/8191*(MAX_ACTUATOR_LENGTH/dblActuatorConversion[0])/1000;
WD40andTape 3:c83291bf9fd2 239
WD40andTape 3:c83291bf9fd2 240
dofydoink 0:607bc887b6e0 241 }
dofydoink 0:607bc887b6e0 242
dofydoink 0:607bc887b6e0 243 //common rise handler function
dofydoink 0:607bc887b6e0 244
dofydoink 0:607bc887b6e0 245 void common_rise_handler(int channel)
dofydoink 0:607bc887b6e0 246 {
WD40andTape 3:c83291bf9fd2 247 pinCheck = 1;
dofydoink 0:607bc887b6e0 248 //check if data is ready for tranmission
WD40andTape 1:2a43cf183a62 249 if (isDataReady[channel])
dofydoink 0:607bc887b6e0 250 {
WD40andTape 3:c83291bf9fd2 251
dofydoink 0:607bc887b6e0 252 // Add transmit task to event queue
WD40andTape 1:2a43cf183a62 253 ThreadID[channel] = queue.call(SendReceiveData,channel);//schedule transmission
dofydoink 0:607bc887b6e0 254 }
dofydoink 0:607bc887b6e0 255 }
dofydoink 0:607bc887b6e0 256
WD40andTape 3:c83291bf9fd2 257 void testRiseFunction(void)
WD40andTape 3:c83291bf9fd2 258 {
WD40andTape 3:c83291bf9fd2 259 pinCheck = 1;
WD40andTape 3:c83291bf9fd2 260 //check if data is ready for tranmission
WD40andTape 3:c83291bf9fd2 261 if (isDataReady[0])
WD40andTape 3:c83291bf9fd2 262 {
WD40andTape 3:c83291bf9fd2 263 // Add transmit task to event queue
WD40andTape 3:c83291bf9fd2 264 ThreadID[0] = queue.call(testSendReceiveData);//schedule transmission
WD40andTape 3:c83291bf9fd2 265 }
WD40andTape 3:c83291bf9fd2 266 }
WD40andTape 3:c83291bf9fd2 267
WD40andTape 3:c83291bf9fd2 268 void testFallFunction(void)
WD40andTape 3:c83291bf9fd2 269 {
WD40andTape 3:c83291bf9fd2 270 pinCheck = 0;
WD40andTape 3:c83291bf9fd2 271 //cancel relevant queued event
WD40andTape 3:c83291bf9fd2 272 queue.cancel(ThreadID[0]);
WD40andTape 3:c83291bf9fd2 273 }
WD40andTape 3:c83291bf9fd2 274
dofydoink 0:607bc887b6e0 275
dofydoink 0:607bc887b6e0 276
dofydoink 0:607bc887b6e0 277 //common_fall handler functions
dofydoink 0:607bc887b6e0 278 void common_fall_handler(int channel)
dofydoink 0:607bc887b6e0 279 {
WD40andTape 3:c83291bf9fd2 280 pinCheck = 0;
dofydoink 0:607bc887b6e0 281 //cancel relevant queued event
dofydoink 0:607bc887b6e0 282 queue.cancel(ThreadID[channel]);
dofydoink 0:607bc887b6e0 283 }
dofydoink 0:607bc887b6e0 284
dofydoink 0:607bc887b6e0 285 //stub rise functions
dofydoink 0:607bc887b6e0 286 void rise0(void) { common_rise_handler(0); }
dofydoink 0:607bc887b6e0 287 void rise1(void) { common_rise_handler(1); }
dofydoink 0:607bc887b6e0 288 void rise2(void) { common_rise_handler(2); }
dofydoink 0:607bc887b6e0 289 void rise3(void) { common_rise_handler(3); }
dofydoink 0:607bc887b6e0 290 void rise4(void) { common_rise_handler(4); }
dofydoink 0:607bc887b6e0 291 void rise5(void) { common_rise_handler(5); }
dofydoink 0:607bc887b6e0 292 void rise6(void) { common_rise_handler(6); }
dofydoink 0:607bc887b6e0 293 void rise7(void) { common_rise_handler(7); }
dofydoink 0:607bc887b6e0 294
dofydoink 0:607bc887b6e0 295 //stub fall functions
dofydoink 0:607bc887b6e0 296 void fall0(void) { common_fall_handler(0); }
dofydoink 0:607bc887b6e0 297 void fall1(void) { common_fall_handler(1); }
dofydoink 0:607bc887b6e0 298 void fall2(void) { common_fall_handler(2); }
dofydoink 0:607bc887b6e0 299 void fall3(void) { common_fall_handler(3); }
dofydoink 0:607bc887b6e0 300 void fall4(void) { common_fall_handler(4); }
dofydoink 0:607bc887b6e0 301 void fall5(void) { common_fall_handler(5); }
dofydoink 0:607bc887b6e0 302 void fall6(void) { common_fall_handler(6); }
dofydoink 0:607bc887b6e0 303 void fall7(void) { common_fall_handler(7); }
dofydoink 0:607bc887b6e0 304
WD40andTape 1:2a43cf183a62 305 void startPathPlan() // Plan a new linear path after receiving new target data
dofydoink 0:607bc887b6e0 306 {
WD40andTape 1:2a43cf183a62 307 semPathPlan.release(); // Uses threadReplan which is below normal priority to ensure consistent transmission to LL
dofydoink 0:607bc887b6e0 308 }
dofydoink 0:607bc887b6e0 309
dofydoink 0:607bc887b6e0 310 //this function will be called when a new transmission is received from high level
dofydoink 0:607bc887b6e0 311 void ReplanPath()
dofydoink 0:607bc887b6e0 312 {
WD40andTape 2:eea12b149dba 313
dofydoink 0:607bc887b6e0 314 //while(1)
dofydoink 0:607bc887b6e0 315 //{
dofydoink 0:607bc887b6e0 316 //semReplan.wait();//wait until called
dofydoink 0:607bc887b6e0 317 //printf("replan!\r\n");
WD40andTape 1:2a43cf183a62 318 //for (ii = 0; ii < N_CHANNELS; ii++)
WD40andTape 1:2a43cf183a62 319 // {
WD40andTape 1:2a43cf183a62 320 // mutChannel[ii].lock();
WD40andTape 1:2a43cf183a62 321 // }
WD40andTape 1:2a43cf183a62 322
WD40andTape 1:2a43cf183a62 323 mutPsi.lock();// lock mutex for PSI to ensure no conflict when receiving new messages while already replanning
WD40andTape 2:eea12b149dba 324 //!!!!!!!!!!!!!!! If received messages faster than path replanning, will get increasingly behind time --> need to throw away some instructions
dofydoink 0:607bc887b6e0 325
WD40andTape 2:eea12b149dba 326 double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
dofydoink 0:607bc887b6e0 327 //update front segment
dofydoink 0:607bc887b6e0 328 dblTargetChambLen_mm[0] = dblPSI[0][0]*1000;
dofydoink 0:607bc887b6e0 329 dblTargetChambLen_mm[1] = dblPSI[0][1]*1000;
dofydoink 0:607bc887b6e0 330 dblTargetChambLen_mm[2] = dblPSI[0][2]*1000;
dofydoink 0:607bc887b6e0 331 //update mid segment
dofydoink 0:607bc887b6e0 332 dblTargetChambLen_mm[6] = dblPSI[1][0]*1000;
dofydoink 0:607bc887b6e0 333 dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; //same because two pumps are used
dofydoink 0:607bc887b6e0 334 //update rear segment
dofydoink 0:607bc887b6e0 335 dblTargetChambLen_mm[3] = dblPSI[2][0]*1000;
dofydoink 0:607bc887b6e0 336 dblTargetChambLen_mm[4] = dblPSI[2][1]*1000;
dofydoink 0:607bc887b6e0 337 dblTargetChambLen_mm[5] = dblPSI[2][2]*1000;
WD40andTape 2:eea12b149dba 338 /* dblTargetChambLen_mm = { psi.
WD40andTape 2:eea12b149dba 339
WD40andTape 2:eea12b149dba 340 dblTargetTime_s*/
dofydoink 0:607bc887b6e0 341
WD40andTape 1:2a43cf183a62 342 mutPsi.unlock();// unlock mutex for PSI to ensure no conflict when receiving new messages while already replanning
WD40andTape 1:2a43cf183a62 343
WD40andTape 1:2a43cf183a62 344 // for (ii = 0; ii < N_CHANNELS; ii++)
WD40andTape 1:2a43cf183a62 345 // {
WD40andTape 1:2a43cf183a62 346 // mutChannel[ii].unlock();
WD40andTape 1:2a43cf183a62 347 // }
dofydoink 0:607bc887b6e0 348
dofydoink 0:607bc887b6e0 349 for (ii = 0; ii< N_CHANNELS; ii++)
dofydoink 0:607bc887b6e0 350 {
WD40andTape 1:2a43cf183a62 351 mutChannel[ii].lock();//lock variables to avoid race condition
dofydoink 0:607bc887b6e0 352
dofydoink 0:607bc887b6e0 353 //check to see if positions are achievable
dofydoink 0:607bc887b6e0 354 if(dblTargetChambLen_mm[ii]>dblMaxChamberLengths_mm[ii])
dofydoink 0:607bc887b6e0 355 {
dofydoink 0:607bc887b6e0 356 dblTargetChambLen_mm[ii] = dblMaxChamberLengths_mm[ii];
dofydoink 0:607bc887b6e0 357 }
dofydoink 0:607bc887b6e0 358
dofydoink 0:607bc887b6e0 359 if(dblTargetChambLen_mm[ii]<0.0)
dofydoink 0:607bc887b6e0 360 {
dofydoink 0:607bc887b6e0 361 dblTargetChambLen_mm[ii] = 0.0;
dofydoink 0:607bc887b6e0 362 }
dofydoink 0:607bc887b6e0 363
dofydoink 0:607bc887b6e0 364 dblTargetActLen_mm[ii] = dblTargetChambLen_mm[ii]*dblActuatorConversion[ii];
dofydoink 0:607bc887b6e0 365
dofydoink 0:607bc887b6e0 366 //work out new velocities
WD40andTape 2:eea12b149dba 367 dblVelocity_mmps[ii] = (dblTargetActLen_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblTargetTime_s;
dofydoink 0:607bc887b6e0 368
dofydoink 0:607bc887b6e0 369 //check to see if velocities are achievable
dofydoink 0:607bc887b6e0 370 if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS)
dofydoink 0:607bc887b6e0 371 {
dofydoink 0:607bc887b6e0 372 if(dblVelocity_mmps[ii]>0)
dofydoink 0:607bc887b6e0 373 {
dofydoink 0:607bc887b6e0 374 dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
dofydoink 0:607bc887b6e0 375 }
dofydoink 0:607bc887b6e0 376 else
dofydoink 0:607bc887b6e0 377 {
dofydoink 0:607bc887b6e0 378 dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
dofydoink 0:607bc887b6e0 379 }
dofydoink 0:607bc887b6e0 380 }
dofydoink 0:607bc887b6e0 381
WD40andTape 1:2a43cf183a62 382 mutChannel[ii].unlock(); //unlock mutex.
dofydoink 0:607bc887b6e0 383 }
dofydoink 0:607bc887b6e0 384 //}
dofydoink 0:607bc887b6e0 385
dofydoink 0:607bc887b6e0 386
dofydoink 0:607bc887b6e0 387 }
dofydoink 0:607bc887b6e0 388
WD40andTape 2:eea12b149dba 389 // lock mutex
WD40andTape 2:eea12b149dba 390 // get variables
WD40andTape 2:eea12b149dba 391 // unlock mutex
WD40andTape 2:eea12b149dba 392 // do calculations
WD40andTape 2:eea12b149dba 393 // lock mutex
WD40andTape 2:eea12b149dba 394 // set variables
WD40andTape 2:eea12b149dba 395 // unlock mutex
WD40andTape 2:eea12b149dba 396
WD40andTape 1:2a43cf183a62 397 void CalculateSmoothPath()
dofydoink 0:607bc887b6e0 398 {
WD40andTape 1:2a43cf183a62 399 double dblMeasuredSampleTime;
dofydoink 0:607bc887b6e0 400 while(1)
dofydoink 0:607bc887b6e0 401 {
dofydoink 0:607bc887b6e0 402 semPathPlan.wait();
dofydoink 0:607bc887b6e0 403 //if(blnReplan)
dofydoink 0:607bc887b6e0 404 // {
dofydoink 0:607bc887b6e0 405 // blnReplan = 0;//remove flag
dofydoink 0:607bc887b6e0 406 // ReplanPath(dblPSI, dblTargetTime_s);
dofydoink 0:607bc887b6e0 407 // }
WD40andTape 1:2a43cf183a62 408
WD40andTape 1:2a43cf183a62 409 // If run time is more than 50 us from expected, calculate from measured time step
WD40andTape 1:2a43cf183a62 410 if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50)
WD40andTape 1:2a43cf183a62 411 {
WD40andTape 1:2a43cf183a62 412 dblMeasuredSampleTime = timer.read();
WD40andTape 1:2a43cf183a62 413 }
WD40andTape 1:2a43cf183a62 414 else
WD40andTape 1:2a43cf183a62 415 {
WD40andTape 1:2a43cf183a62 416 dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
WD40andTape 1:2a43cf183a62 417 }
WD40andTape 1:2a43cf183a62 418 timer.reset();
WD40andTape 1:2a43cf183a62 419
dofydoink 0:607bc887b6e0 420 for(ii = 0; ii < N_CHANNELS; ii++)
dofydoink 0:607bc887b6e0 421 {
dofydoink 0:607bc887b6e0 422 //calculate next step in linear path
WD40andTape 1:2a43cf183a62 423 mutChannel[ii].lock();//lock relevant mutex
WD40andTape 2:eea12b149dba 424 dblLinearPathCurrentPos_mm[ii] = dblLinearPathCurrentPos_mm[ii] + dblVelocity_mmps[ii]*dblMeasuredSampleTime; // PATH_SAMPLE_TIME_S SHOULD BE MEASURED
WD40andTape 1:2a43cf183a62 425
dofydoink 0:607bc887b6e0 426 //check tolerance
WD40andTape 2:eea12b149dba 427 if (fabs(dblLinearPathCurrentPos_mm[ii] - dblTargetActLen_mm[ii]) <= dblPathTolerance)
dofydoink 0:607bc887b6e0 428 {
dofydoink 0:607bc887b6e0 429 dblVelocity_mmps[ii] = 0; //stop linear path generation when linear path is within tolerance of target position.
dofydoink 0:607bc887b6e0 430 }
WD40andTape 1:2a43cf183a62 431 mutChannel[ii].unlock();//unlock relevant mutex
dofydoink 0:607bc887b6e0 432
dofydoink 0:607bc887b6e0 433 //calculate next step in smooth path
WD40andTape 2:eea12b149dba 434 dblSmoothPathCurrentPos_mm[ii] = dblSmoothingFactor*dblLinearPathCurrentPos_mm[ii] + (1.0-dblSmoothingFactor)*dblSmoothPathCurrentPos_mm[ii];
dofydoink 0:607bc887b6e0 435
dofydoink 0:607bc887b6e0 436 //convert to actuator distances
WD40andTape 2:eea12b149dba 437 dblPathToActuator[ii] = dblSmoothPathCurrentPos_mm[ii];
dofydoink 0:607bc887b6e0 438
WD40andTape 3:c83291bf9fd2 439 intDemandPos_Tx[ii] = (int) ((dblPathToActuator[ii]/MAX_ACTUATOR_LENGTH)*8191);//convert to a 13-bit number;
WD40andTape 3:c83291bf9fd2 440
WD40andTape 1:2a43cf183a62 441 intDemandPos_Tx[ii] = intDemandPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit
dofydoink 0:607bc887b6e0 442
WD40andTape 3:c83291bf9fd2 443 dblPressure_bar[ii] = ReadADCPressure(ii);
WD40andTape 3:c83291bf9fd2 444 dblPosition_mtrs[ii] = ReadADCPosition(ii);
dofydoink 0:607bc887b6e0 445
WD40andTape 1:2a43cf183a62 446 isDataReady[ii] = 1;//signal that data ready
dofydoink 0:607bc887b6e0 447
dofydoink 0:607bc887b6e0 448 }
WD40andTape 3:c83291bf9fd2 449 printf("%f, %d\r\n",dblPathToActuator[0], intDemandPos_Tx[0]);
WD40andTape 3:c83291bf9fd2 450 //printf("%f,%f, %f, %f\r\n", dblTargetActLen_mm[0], dblSmoothPathCurrentPos_mm[0],dblPressure_bar[0], dblPosition_mtrs[0]);
dofydoink 0:607bc887b6e0 451 }
dofydoink 0:607bc887b6e0 452 }
dofydoink 0:607bc887b6e0 453
WD40andTape 3:c83291bf9fd2 454 void SimulateDemand() // For testing purposes
dofydoink 0:607bc887b6e0 455 {
WD40andTape 2:eea12b149dba 456 int kk = 0;
dofydoink 0:607bc887b6e0 457 while(1)
dofydoink 0:607bc887b6e0 458 {
WD40andTape 1:2a43cf183a62 459 mutChannel[0].lock();
dofydoink 0:607bc887b6e0 460 if(kk == 0)
dofydoink 0:607bc887b6e0 461 {
dofydoink 0:607bc887b6e0 462 dblPSI[0][0] = (double) 10.0;
WD40andTape 3:c83291bf9fd2 463 dblTargetTime_s = 1.5;
dofydoink 0:607bc887b6e0 464 }
dofydoink 0:607bc887b6e0 465 else
dofydoink 0:607bc887b6e0 466 {
dofydoink 0:607bc887b6e0 467 dblPSI[0][0] = (double) 0.0;
dofydoink 0:607bc887b6e0 468 dblTargetTime_s = 2.0;
dofydoink 0:607bc887b6e0 469 }
dofydoink 0:607bc887b6e0 470
dofydoink 0:607bc887b6e0 471 kk = 1 - kk;
dofydoink 0:607bc887b6e0 472
dofydoink 0:607bc887b6e0 473
dofydoink 0:607bc887b6e0 474 //semReplan.release();
dofydoink 0:607bc887b6e0 475
WD40andTape 1:2a43cf183a62 476 mutChannel[0].unlock();
dofydoink 0:607bc887b6e0 477 ReplanPath();
dofydoink 0:607bc887b6e0 478
dofydoink 0:607bc887b6e0 479 Thread::wait(7000);
dofydoink 0:607bc887b6e0 480 }
WD40andTape 3:c83291bf9fd2 481 }
dofydoink 0:607bc887b6e0 482
dofydoink 0:607bc887b6e0 483
dofydoink 0:607bc887b6e0 484
dofydoink 0:607bc887b6e0 485 Ticker PathCalculationTicker;
dofydoink 0:607bc887b6e0 486
dofydoink 0:607bc887b6e0 487 int main()
dofydoink 0:607bc887b6e0 488 {
WD40andTape 3:c83291bf9fd2 489 for (ii = 0; ii <250; ii++)
WD40andTape 3:c83291bf9fd2 490 {
WD40andTape 3:c83291bf9fd2 491 pinCheck = 1;
WD40andTape 3:c83291bf9fd2 492 wait(0.001);
WD40andTape 3:c83291bf9fd2 493 pinCheck = 0;
WD40andTape 3:c83291bf9fd2 494 wait(0.001);
WD40andTape 3:c83291bf9fd2 495 }
WD40andTape 3:c83291bf9fd2 496
dofydoink 0:607bc887b6e0 497 //initialise relevant variables
dofydoink 0:607bc887b6e0 498 for(ii = 0; ii<N_CHANNELS; ii++)
dofydoink 0:607bc887b6e0 499 {
WD40andTape 3:c83291bf9fd2 500
dofydoink 0:607bc887b6e0 501 //all chip selects in off state
dofydoink 0:607bc887b6e0 502 cs_LL[ii] = 1;
dofydoink 0:607bc887b6e0 503 cs_ADC[ii] = 1;
dofydoink 0:607bc887b6e0 504
dofydoink 0:607bc887b6e0 505 //data ready flags set to not ready
WD40andTape 1:2a43cf183a62 506 isDataReady[ii] = 0;
dofydoink 0:607bc887b6e0 507 }
dofydoink 0:607bc887b6e0 508
dofydoink 0:607bc887b6e0 509 //calculate some control variables
WD40andTape 1:2a43cf183a62 510 dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance in mm.
dofydoink 0:607bc887b6e0 511
dofydoink 0:607bc887b6e0 512 pinReset = 1; //initialise reset pin to not reset the controllers.
WD40andTape 3:c83291bf9fd2 513 wait(0.1);
WD40andTape 3:c83291bf9fd2 514 pinReset=0; //reset controllers to be safe
WD40andTape 3:c83291bf9fd2 515 wait(0.1);
WD40andTape 3:c83291bf9fd2 516 pinReset = 1;//ready to go
dofydoink 0:607bc887b6e0 517
dofydoink 0:607bc887b6e0 518 //say something nice to the user.
dofydoink 0:607bc887b6e0 519 pc.baud(9600);
dofydoink 0:607bc887b6e0 520 printf("Hi, there! I'll be your mid-level controller for today.\r\n");
dofydoink 0:607bc887b6e0 521
dofydoink 0:607bc887b6e0 522 // Start the event queue
dofydoink 0:607bc887b6e0 523 t.start(callback(&queue, &EventQueue::dispatch_forever));
dofydoink 0:607bc887b6e0 524
dofydoink 0:607bc887b6e0 525 //set up the interrupts
dofydoink 0:607bc887b6e0 526
dofydoink 0:607bc887b6e0 527 //set up rise interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 3:c83291bf9fd2 528 //
WD40andTape 3:c83291bf9fd2 529 //pinGate[0].rise(&testRiseFunction);
WD40andTape 3:c83291bf9fd2 530 //testPinGate.rise(&testRiseFunction);//<working
WD40andTape 3:c83291bf9fd2 531 //testPinGate.rise(&rise0);
WD40andTape 3:c83291bf9fd2 532
WD40andTape 3:c83291bf9fd2 533 pinGate0.rise(&rise0);
WD40andTape 3:c83291bf9fd2 534 /*
WD40andTape 3:c83291bf9fd2 535 pinGate1.rise(&rise1);
WD40andTape 3:c83291bf9fd2 536 pinGate2.rise(&rise2);
WD40andTape 3:c83291bf9fd2 537 pinGate3.rise(&rise3);
WD40andTape 3:c83291bf9fd2 538 pinGate4.rise(&rise4);
WD40andTape 3:c83291bf9fd2 539 pinGate5.rise(&rise5);
WD40andTape 3:c83291bf9fd2 540 pinGate6.rise(&rise6);
WD40andTape 3:c83291bf9fd2 541 pinGate7.rise(&rise7);
WD40andTape 3:c83291bf9fd2 542 */
dofydoink 0:607bc887b6e0 543
dofydoink 0:607bc887b6e0 544 //set up fall interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 3:c83291bf9fd2 545 //
WD40andTape 3:c83291bf9fd2 546 //pinGate[0].fall(&testFallFunction);
WD40andTape 3:c83291bf9fd2 547 //testPinGate.fall(&testFallFunction); <working
WD40andTape 3:c83291bf9fd2 548 //testPinGate.fall(&fall0);
WD40andTape 3:c83291bf9fd2 549
WD40andTape 3:c83291bf9fd2 550 pinGate0.fall(&fall0);
WD40andTape 3:c83291bf9fd2 551 /*
WD40andTape 3:c83291bf9fd2 552 pinGate1.fall(&fall1);
WD40andTape 3:c83291bf9fd2 553 pinGate2.fall(&fall2);
WD40andTape 3:c83291bf9fd2 554 pinGate3.fall(&fall3);
WD40andTape 3:c83291bf9fd2 555 pinGate4.fall(&fall4);
WD40andTape 3:c83291bf9fd2 556 pinGate5.fall(&fall5);
WD40andTape 3:c83291bf9fd2 557 pinGate6.fall(&fall6);
WD40andTape 3:c83291bf9fd2 558 pinGate7.fall(&fall7);
WD40andTape 3:c83291bf9fd2 559 */
dofydoink 0:607bc887b6e0 560
dofydoink 0:607bc887b6e0 561 timer.start();
dofydoink 0:607bc887b6e0 562
dofydoink 0:607bc887b6e0 563 threadSimulateDemand.start(SimulateDemand);
WD40andTape 1:2a43cf183a62 564 threadPathPlan.start(CalculateSmoothPath); //start planning thread
WD40andTape 1:2a43cf183a62 565
dofydoink 0:607bc887b6e0 566 PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); //set up planning thread to recur at fixed intervals
WD40andTape 1:2a43cf183a62 567
dofydoink 0:607bc887b6e0 568 threadReplan.start(ReplanPath);//start Replanning thread
dofydoink 0:607bc887b6e0 569 Thread::wait(1);
WD40andTape 3:c83291bf9fd2 570
dofydoink 0:607bc887b6e0 571
WD40andTape 1:2a43cf183a62 572 while(1) {
WD40andTape 1:2a43cf183a62 573 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 574 }
dofydoink 0:607bc887b6e0 575 }
dofydoink 0:607bc887b6e0 576