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

Committer:
WD40andTape
Date:
Tue Jul 31 17:54:29 2018 +0000
Revision:
4:303584310071
Parent:
3:c83291bf9fd2
Child:
5:712e7634c779
cba

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
WD40andTape 4:303584310071 36 const double DBL_MAX_CHAMBER_LENGTHS_MM[N_CHANNELS] = {100.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
WD40andTape 4:303584310071 103
WD40andTape 4:303584310071 104 double ReadADCPosition_mtrs(int channel)
dofydoink 0:607bc887b6e0 105 {
dofydoink 0:607bc887b6e0 106 unsigned int outputA;
dofydoink 0:607bc887b6e0 107 unsigned int outputB;
WD40andTape 3:c83291bf9fd2 108 int output;
WD40andTape 3:c83291bf9fd2 109 double dblOutput;
dofydoink 0:607bc887b6e0 110
dofydoink 0:607bc887b6e0 111 spi.format(8,0);
dofydoink 0:607bc887b6e0 112 spi.frequency(1000000);
dofydoink 0:607bc887b6e0 113
dofydoink 0:607bc887b6e0 114 cs_ADC[channel] = 0;
dofydoink 0:607bc887b6e0 115 spi.write(PREAMBLE);
dofydoink 0:607bc887b6e0 116 outputA = spi.write(CHAN_3);
dofydoink 0:607bc887b6e0 117 outputB = spi.write(0xFF);
WD40andTape 1:2a43cf183a62 118 cs_ADC[channel]= 1;
dofydoink 0:607bc887b6e0 119
dofydoink 0:607bc887b6e0 120 outputA = outputA & DATA_MASK;
dofydoink 0:607bc887b6e0 121 outputA = outputA<<8;
dofydoink 0:607bc887b6e0 122 output = (outputA | outputB);
WD40andTape 3:c83291bf9fd2 123 output = 4095- output;
WD40andTape 4:303584310071 124 dblOutput = (double) ((output - 1504)/4095*0.1);
WD40andTape 3:c83291bf9fd2 125 return dblOutput;
dofydoink 0:607bc887b6e0 126 }
dofydoink 0:607bc887b6e0 127
WD40andTape 4:303584310071 128 double ReadADCPressure_bar(int channel)
dofydoink 0:607bc887b6e0 129 {
dofydoink 0:607bc887b6e0 130 unsigned int outputA;
dofydoink 0:607bc887b6e0 131 unsigned int outputB;
WD40andTape 3:c83291bf9fd2 132 int output;
WD40andTape 3:c83291bf9fd2 133 double dblOutput;
dofydoink 0:607bc887b6e0 134
dofydoink 0:607bc887b6e0 135 spi.format(8,0);
dofydoink 0:607bc887b6e0 136 spi.frequency(1000000);
dofydoink 0:607bc887b6e0 137
dofydoink 0:607bc887b6e0 138 cs_ADC[channel] = 0;
dofydoink 0:607bc887b6e0 139 spi.write(PREAMBLE);
dofydoink 0:607bc887b6e0 140 outputA = spi.write(CHAN_1);
dofydoink 0:607bc887b6e0 141 outputB = spi.write(0xFF);
dofydoink 0:607bc887b6e0 142 cs_ADC[channel] = 1;
dofydoink 0:607bc887b6e0 143
dofydoink 0:607bc887b6e0 144 outputA = outputA & DATA_MASK;
dofydoink 0:607bc887b6e0 145 outputA = outputA<<8;
dofydoink 0:607bc887b6e0 146 output = (outputA | outputB);
dofydoink 0:607bc887b6e0 147
WD40andTape 4:303584310071 148 dblOutput = (double) ((output-502)/4095*8.0);
WD40andTape 3:c83291bf9fd2 149 return dblOutput;
dofydoink 0:607bc887b6e0 150 }
dofydoink 0:607bc887b6e0 151
WD40andTape 1:2a43cf183a62 152 void SendReceiveData(int channel)
dofydoink 0:607bc887b6e0 153 {
WD40andTape 3:c83291bf9fd2 154
WD40andTape 4:303584310071 155
WD40andTape 4:303584310071 156 int intPosSPI_Rx[N_CHANNELS]; //13 bit value received over SPI from the actuator
WD40andTape 4:303584310071 157
WD40andTape 4:303584310071 158
dofydoink 0:607bc887b6e0 159 //get data from controller
dofydoink 0:607bc887b6e0 160 spi.format(16,2);
dofydoink 0:607bc887b6e0 161 spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 4:303584310071 162 mutChannel[channel].lock();//lock mutex for specific Channel
dofydoink 0:607bc887b6e0 163 cs_LL[channel] = 0;//select relevant chip
WD40andTape 1:2a43cf183a62 164 intPosSPI_Rx[channel] = spi.write(intDemandPos_Tx[channel]); //transmit & receive
dofydoink 0:607bc887b6e0 165 cs_LL[channel] = 1;//deselect chip
WD40andTape 1:2a43cf183a62 166 isDataReady[channel] = 0;//data no longer ready, i.e. we now require new data
WD40andTape 1:2a43cf183a62 167
WD40andTape 3:c83291bf9fd2 168
dofydoink 0:607bc887b6e0 169
dofydoink 0:607bc887b6e0 170 //sort out received data
dofydoink 0:607bc887b6e0 171 chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
WD40andTape 4:303584310071 172
dofydoink 0:607bc887b6e0 173 intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
WD40andTape 4:303584310071 174 //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
WD40andTape 4:303584310071 175 mutChannel[channel].unlock();//unlock mutex for specific channel
WD40andTape 3:c83291bf9fd2 176 //printf("%d, %d\r\n",intPosSPI_Rx[0],intDemandPos_Tx[0]);
WD40andTape 3:c83291bf9fd2 177
WD40andTape 3:c83291bf9fd2 178 }
WD40andTape 3:c83291bf9fd2 179
WD40andTape 4:303584310071 180
dofydoink 0:607bc887b6e0 181
dofydoink 0:607bc887b6e0 182 //common rise handler function
dofydoink 0:607bc887b6e0 183
dofydoink 0:607bc887b6e0 184 void common_rise_handler(int channel)
dofydoink 0:607bc887b6e0 185 {
WD40andTape 3:c83291bf9fd2 186 pinCheck = 1;
dofydoink 0:607bc887b6e0 187 //check if data is ready for tranmission
WD40andTape 1:2a43cf183a62 188 if (isDataReady[channel])
dofydoink 0:607bc887b6e0 189 {
dofydoink 0:607bc887b6e0 190 // Add transmit task to event queue
WD40andTape 1:2a43cf183a62 191 ThreadID[channel] = queue.call(SendReceiveData,channel);//schedule transmission
dofydoink 0:607bc887b6e0 192 }
dofydoink 0:607bc887b6e0 193 }
dofydoink 0:607bc887b6e0 194
WD40andTape 3:c83291bf9fd2 195
dofydoink 0:607bc887b6e0 196
dofydoink 0:607bc887b6e0 197
dofydoink 0:607bc887b6e0 198 //common_fall handler functions
dofydoink 0:607bc887b6e0 199 void common_fall_handler(int channel)
dofydoink 0:607bc887b6e0 200 {
WD40andTape 3:c83291bf9fd2 201 pinCheck = 0;
dofydoink 0:607bc887b6e0 202 //cancel relevant queued event
dofydoink 0:607bc887b6e0 203 queue.cancel(ThreadID[channel]);
dofydoink 0:607bc887b6e0 204 }
dofydoink 0:607bc887b6e0 205
dofydoink 0:607bc887b6e0 206 //stub rise functions
dofydoink 0:607bc887b6e0 207 void rise0(void) { common_rise_handler(0); }
dofydoink 0:607bc887b6e0 208 void rise1(void) { common_rise_handler(1); }
dofydoink 0:607bc887b6e0 209 void rise2(void) { common_rise_handler(2); }
dofydoink 0:607bc887b6e0 210 void rise3(void) { common_rise_handler(3); }
dofydoink 0:607bc887b6e0 211 void rise4(void) { common_rise_handler(4); }
dofydoink 0:607bc887b6e0 212 void rise5(void) { common_rise_handler(5); }
dofydoink 0:607bc887b6e0 213 void rise6(void) { common_rise_handler(6); }
dofydoink 0:607bc887b6e0 214 void rise7(void) { common_rise_handler(7); }
dofydoink 0:607bc887b6e0 215
dofydoink 0:607bc887b6e0 216 //stub fall functions
dofydoink 0:607bc887b6e0 217 void fall0(void) { common_fall_handler(0); }
dofydoink 0:607bc887b6e0 218 void fall1(void) { common_fall_handler(1); }
dofydoink 0:607bc887b6e0 219 void fall2(void) { common_fall_handler(2); }
dofydoink 0:607bc887b6e0 220 void fall3(void) { common_fall_handler(3); }
dofydoink 0:607bc887b6e0 221 void fall4(void) { common_fall_handler(4); }
dofydoink 0:607bc887b6e0 222 void fall5(void) { common_fall_handler(5); }
dofydoink 0:607bc887b6e0 223 void fall6(void) { common_fall_handler(6); }
dofydoink 0:607bc887b6e0 224 void fall7(void) { common_fall_handler(7); }
dofydoink 0:607bc887b6e0 225
WD40andTape 1:2a43cf183a62 226 void startPathPlan() // Plan a new linear path after receiving new target data
dofydoink 0:607bc887b6e0 227 {
WD40andTape 4:303584310071 228 semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
dofydoink 0:607bc887b6e0 229 }
dofydoink 0:607bc887b6e0 230
dofydoink 0:607bc887b6e0 231 //this function will be called when a new transmission is received from high level
WD40andTape 4:303584310071 232 void ReceiveAndReplan()
dofydoink 0:607bc887b6e0 233 {
WD40andTape 4:303584310071 234 int ii;
WD40andTape 4:303584310071 235
WD40andTape 4:303584310071 236 double dblPSI[3][3]; //the message from high-level containing the chamber lengths in meters in following format:
WD40andTape 4:303584310071 237 /*
WD40andTape 4:303584310071 238 {L(front,1), L(front,2), L(front,3);
WD40andTape 4:303584310071 239 L(mid,1) , L(mid,2) , L(mid,3);
WD40andTape 4:303584310071 240 L(rear,1) , L(rear,2) , L(rear,3);}
WD40andTape 4:303584310071 241 */
WD40andTape 4:303584310071 242 double dblTargetTime_s; //the time in seconds desired to reach the target (>0...!)
WD40andTape 4:303584310071 243
WD40andTape 4:303584310071 244 double dblPosition_mtrs[N_CHANNELS]; //the actual chamber lengths in meters given as the change in length relative to neutral (should always be >=0)
WD40andTape 4:303584310071 245 double dblPressure_bar[N_CHANNELS]; //the pressure in a given chamber in bar (1 bar = 100,000 Pa).
WD40andTape 4:303584310071 246
WD40andTape 4:303584310071 247 //!!!!!!!!!!!!!!! If received messages faster than path replanning, will get increasingly behind time --> need to throw away some instructions
WD40andTape 2:eea12b149dba 248
WD40andTape 4:303584310071 249 //receive
WD40andTape 4:303584310071 250
WD40andTape 4:303584310071 251 double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
WD40andTape 4:303584310071 252
WD40andTape 4:303584310071 253 bool kk = 0;
WD40andTape 4:303584310071 254 while(1) {
WD40andTape 1:2a43cf183a62 255
WD40andTape 4:303584310071 256 if(kk == 0) {
WD40andTape 4:303584310071 257 dblPSI[0][0] = (double) 10.0;
WD40andTape 4:303584310071 258 dblTargetTime_s = 1.5;
WD40andTape 4:303584310071 259 } else {
WD40andTape 4:303584310071 260 dblPSI[0][0] = (double) 0.0;
WD40andTape 4:303584310071 261 dblTargetTime_s = 2.0;
WD40andTape 4:303584310071 262 }
WD40andTape 4:303584310071 263 kk = !kk;
WD40andTape 4:303584310071 264 printf("REPLAN, %f,\r\n", dblPSI[0][0] );
dofydoink 0:607bc887b6e0 265 //update front segment
dofydoink 0:607bc887b6e0 266 dblTargetChambLen_mm[0] = dblPSI[0][0]*1000;
dofydoink 0:607bc887b6e0 267 dblTargetChambLen_mm[1] = dblPSI[0][1]*1000;
dofydoink 0:607bc887b6e0 268 dblTargetChambLen_mm[2] = dblPSI[0][2]*1000;
dofydoink 0:607bc887b6e0 269 //update mid segment
dofydoink 0:607bc887b6e0 270 dblTargetChambLen_mm[6] = dblPSI[1][0]*1000;
dofydoink 0:607bc887b6e0 271 dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; //same because two pumps are used
dofydoink 0:607bc887b6e0 272 //update rear segment
dofydoink 0:607bc887b6e0 273 dblTargetChambLen_mm[3] = dblPSI[2][0]*1000;
dofydoink 0:607bc887b6e0 274 dblTargetChambLen_mm[4] = dblPSI[2][1]*1000;
dofydoink 0:607bc887b6e0 275 dblTargetChambLen_mm[5] = dblPSI[2][2]*1000;
dofydoink 0:607bc887b6e0 276
WD40andTape 4:303584310071 277 for (ii = 0; ii< N_CHANNELS; ii++) {
dofydoink 0:607bc887b6e0 278 //check to see if positions are achievable
WD40andTape 4:303584310071 279 if(dblTargetChambLen_mm[ii]>DBL_MAX_CHAMBER_LENGTHS_MM[ii]) {
WD40andTape 4:303584310071 280 dblTargetChambLen_mm[ii] = DBL_MAX_CHAMBER_LENGTHS_MM[ii];
dofydoink 0:607bc887b6e0 281 }
WD40andTape 4:303584310071 282 if(dblTargetChambLen_mm[ii]<0.0) {
dofydoink 0:607bc887b6e0 283 dblTargetChambLen_mm[ii] = 0.0;
dofydoink 0:607bc887b6e0 284 }
dofydoink 0:607bc887b6e0 285
WD40andTape 4:303584310071 286 //mutPathIn.lock();//lock variables to avoid race condition
WD40andTape 4:303584310071 287
WD40andTape 4:303584310071 288 dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
WD40andTape 4:303584310071 289
dofydoink 0:607bc887b6e0 290 //work out new velocities
WD40andTape 4:303584310071 291 dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblTargetTime_s;
dofydoink 0:607bc887b6e0 292
dofydoink 0:607bc887b6e0 293 //check to see if velocities are achievable
WD40andTape 4:303584310071 294 if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
WD40andTape 4:303584310071 295 if(dblVelocity_mmps[ii]>0) {
dofydoink 0:607bc887b6e0 296 dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
WD40andTape 4:303584310071 297 } else {
dofydoink 0:607bc887b6e0 298 dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
dofydoink 0:607bc887b6e0 299 }
dofydoink 0:607bc887b6e0 300 }
WD40andTape 4:303584310071 301 //mutPathIn.unlock(); //unlock mutex.
dofydoink 0:607bc887b6e0 302
WD40andTape 4:303584310071 303 dblPressure_bar[ii] = ReadADCPressure_bar(ii);//read pressure from channel
WD40andTape 4:303584310071 304 dblPosition_mtrs[ii] = ReadADCPosition_mtrs(ii); //read position from channel
WD40andTape 4:303584310071 305
WD40andTape 4:303584310071 306 //send info back to HL
WD40andTape 4:303584310071 307 } // end of for
WD40andTape 4:303584310071 308 //printf("%f\r\n", dblVelocity_mmps[0]);
WD40andTape 4:303584310071 309 Thread::wait(7000);
WD40andTape 4:303584310071 310 } // end of while(1)
dofydoink 0:607bc887b6e0 311 }
dofydoink 0:607bc887b6e0 312
WD40andTape 2:eea12b149dba 313 // lock mutex
WD40andTape 2:eea12b149dba 314 // get variables
WD40andTape 2:eea12b149dba 315 // unlock mutex
WD40andTape 2:eea12b149dba 316 // do calculations
WD40andTape 2:eea12b149dba 317 // lock mutex
WD40andTape 2:eea12b149dba 318 // set variables
WD40andTape 4:303584310071 319 // unlock mutex
WD40andTape 2:eea12b149dba 320
WD40andTape 4:303584310071 321 void CalculateSmoothPath() {
WD40andTape 4:303584310071 322 int ii;
WD40andTape 1:2a43cf183a62 323 double dblMeasuredSampleTime;
WD40andTape 4:303584310071 324 double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; //the current position of the smooth path (not sent to actuator)
WD40andTape 4:303584310071 325
WD40andTape 4:303584310071 326 while(1) {
dofydoink 0:607bc887b6e0 327 semPathPlan.wait();
WD40andTape 1:2a43cf183a62 328
WD40andTape 1:2a43cf183a62 329 // If run time is more than 50 us from expected, calculate from measured time step
WD40andTape 4:303584310071 330 if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50) {
WD40andTape 1:2a43cf183a62 331 dblMeasuredSampleTime = timer.read();
WD40andTape 4:303584310071 332 } else {
WD40andTape 1:2a43cf183a62 333 dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
WD40andTape 1:2a43cf183a62 334 }
WD40andTape 4:303584310071 335 timer.reset();
WD40andTape 1:2a43cf183a62 336
WD40andTape 4:303584310071 337 for(ii = 0; ii < N_CHANNELS; ii++) {
WD40andTape 4:303584310071 338 if(ii == 0) printf("BEFORE: %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblVelocity_mmps[0],dblMeasuredSampleTime);
dofydoink 0:607bc887b6e0 339 //calculate next step in linear path
WD40andTape 4:303584310071 340 //mutPathIn.lock();//lock relevant mutex
WD40andTape 2:eea12b149dba 341 dblLinearPathCurrentPos_mm[ii] = dblLinearPathCurrentPos_mm[ii] + dblVelocity_mmps[ii]*dblMeasuredSampleTime; // PATH_SAMPLE_TIME_S SHOULD BE MEASURED
WD40andTape 1:2a43cf183a62 342
WD40andTape 4:303584310071 343 if(dblLinearPathCurrentPos_mm[ii] < 0.0) {
WD40andTape 4:303584310071 344 dblLinearPathCurrentPos_mm[ii] = 0.00;
WD40andTape 4:303584310071 345 }
WD40andTape 4:303584310071 346
dofydoink 0:607bc887b6e0 347 //check tolerance
WD40andTape 4:303584310071 348 if (fabs(dblLinearPathCurrentPos_mm[ii] - dblTargetActPos_mm[ii]) <= DBL_PATH_TOLERANCE) {
WD40andTape 4:303584310071 349 dblVelocity_mmps[ii] = 0.0; //stop linear path generation when linear path is within tolerance of target position.
dofydoink 0:607bc887b6e0 350 }
WD40andTape 4:303584310071 351 //mutPathIn.unlock();//unlock relevant mutex
dofydoink 0:607bc887b6e0 352
dofydoink 0:607bc887b6e0 353 //calculate next step in smooth path
WD40andTape 4:303584310071 354 dblSmoothPathCurrentPos_mm[ii] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[ii] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[ii];
WD40andTape 4:303584310071 355 //mutChannel[ii].lock();
WD40andTape 4:303584310071 356 intDemandPos_Tx[ii] = (int) ((dblSmoothPathCurrentPos_mm[ii]/MAX_ACTUATOR_LENGTH)*8191);//convert to a 13-bit number;
WD40andTape 1:2a43cf183a62 357 intDemandPos_Tx[ii] = intDemandPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit
WD40andTape 4:303584310071 358 //mutChannel[ii].unlock();
WD40andTape 4:303584310071 359 if(ii == 0) printf("AFTER: %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[ii],dblVelocity_mmps[ii],dblMeasuredSampleTime);
dofydoink 0:607bc887b6e0 360
WD40andTape 1:2a43cf183a62 361 isDataReady[ii] = 1;//signal that data ready
dofydoink 0:607bc887b6e0 362
WD40andTape 4:303584310071 363 } // end for
WD40andTape 4:303584310071 364 //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
WD40andTape 4:303584310071 365 //printf("%f,%f, %f, %f\r\n", dblTargetActPos_mm[0], dblSmoothPathCurrentPos_mm[0],dblPressure_bar[0], dblPosition_mtrs[0]);
WD40andTape 4:303584310071 366 } // end while
WD40andTape 3:c83291bf9fd2 367 }
dofydoink 0:607bc887b6e0 368
dofydoink 0:607bc887b6e0 369
dofydoink 0:607bc887b6e0 370
dofydoink 0:607bc887b6e0 371 Ticker PathCalculationTicker;
dofydoink 0:607bc887b6e0 372
dofydoink 0:607bc887b6e0 373 int main()
dofydoink 0:607bc887b6e0 374 {
WD40andTape 4:303584310071 375 int ii;
WD40andTape 3:c83291bf9fd2 376
dofydoink 0:607bc887b6e0 377 //initialise relevant variables
dofydoink 0:607bc887b6e0 378 for(ii = 0; ii<N_CHANNELS; ii++)
dofydoink 0:607bc887b6e0 379 {
WD40andTape 3:c83291bf9fd2 380
dofydoink 0:607bc887b6e0 381 //all chip selects in off state
dofydoink 0:607bc887b6e0 382 cs_LL[ii] = 1;
dofydoink 0:607bc887b6e0 383 cs_ADC[ii] = 1;
dofydoink 0:607bc887b6e0 384
dofydoink 0:607bc887b6e0 385 //data ready flags set to not ready
WD40andTape 1:2a43cf183a62 386 isDataReady[ii] = 0;
dofydoink 0:607bc887b6e0 387 }
dofydoink 0:607bc887b6e0 388
dofydoink 0:607bc887b6e0 389 //calculate some control variables
WD40andTape 4:303584310071 390 //dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance in mm.
dofydoink 0:607bc887b6e0 391
dofydoink 0:607bc887b6e0 392 pinReset = 1; //initialise reset pin to not reset the controllers.
WD40andTape 3:c83291bf9fd2 393 wait(0.1);
WD40andTape 3:c83291bf9fd2 394 pinReset=0; //reset controllers to be safe
WD40andTape 3:c83291bf9fd2 395 wait(0.1);
WD40andTape 3:c83291bf9fd2 396 pinReset = 1;//ready to go
dofydoink 0:607bc887b6e0 397
dofydoink 0:607bc887b6e0 398 //say something nice to the user.
dofydoink 0:607bc887b6e0 399 pc.baud(9600);
dofydoink 0:607bc887b6e0 400 printf("Hi, there! I'll be your mid-level controller for today.\r\n");
dofydoink 0:607bc887b6e0 401
dofydoink 0:607bc887b6e0 402 // Start the event queue
dofydoink 0:607bc887b6e0 403 t.start(callback(&queue, &EventQueue::dispatch_forever));
dofydoink 0:607bc887b6e0 404
dofydoink 0:607bc887b6e0 405 //set up the interrupts
dofydoink 0:607bc887b6e0 406
dofydoink 0:607bc887b6e0 407 //set up rise interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 3:c83291bf9fd2 408 //
WD40andTape 3:c83291bf9fd2 409 //pinGate[0].rise(&testRiseFunction);
WD40andTape 3:c83291bf9fd2 410 //testPinGate.rise(&testRiseFunction);//<working
WD40andTape 3:c83291bf9fd2 411 //testPinGate.rise(&rise0);
WD40andTape 3:c83291bf9fd2 412
WD40andTape 3:c83291bf9fd2 413 pinGate0.rise(&rise0);
WD40andTape 3:c83291bf9fd2 414 /*
WD40andTape 3:c83291bf9fd2 415 pinGate1.rise(&rise1);
WD40andTape 3:c83291bf9fd2 416 pinGate2.rise(&rise2);
WD40andTape 3:c83291bf9fd2 417 pinGate3.rise(&rise3);
WD40andTape 3:c83291bf9fd2 418 pinGate4.rise(&rise4);
WD40andTape 3:c83291bf9fd2 419 pinGate5.rise(&rise5);
WD40andTape 3:c83291bf9fd2 420 pinGate6.rise(&rise6);
WD40andTape 3:c83291bf9fd2 421 pinGate7.rise(&rise7);
WD40andTape 3:c83291bf9fd2 422 */
dofydoink 0:607bc887b6e0 423
dofydoink 0:607bc887b6e0 424 //set up fall interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 3:c83291bf9fd2 425 //
WD40andTape 3:c83291bf9fd2 426 //pinGate[0].fall(&testFallFunction);
WD40andTape 3:c83291bf9fd2 427 //testPinGate.fall(&testFallFunction); <working
WD40andTape 3:c83291bf9fd2 428 //testPinGate.fall(&fall0);
WD40andTape 3:c83291bf9fd2 429
WD40andTape 3:c83291bf9fd2 430 pinGate0.fall(&fall0);
WD40andTape 3:c83291bf9fd2 431 /*
WD40andTape 3:c83291bf9fd2 432 pinGate1.fall(&fall1);
WD40andTape 3:c83291bf9fd2 433 pinGate2.fall(&fall2);
WD40andTape 3:c83291bf9fd2 434 pinGate3.fall(&fall3);
WD40andTape 3:c83291bf9fd2 435 pinGate4.fall(&fall4);
WD40andTape 3:c83291bf9fd2 436 pinGate5.fall(&fall5);
WD40andTape 3:c83291bf9fd2 437 pinGate6.fall(&fall6);
WD40andTape 3:c83291bf9fd2 438 pinGate7.fall(&fall7);
WD40andTape 3:c83291bf9fd2 439 */
dofydoink 0:607bc887b6e0 440
dofydoink 0:607bc887b6e0 441 timer.start();
dofydoink 0:607bc887b6e0 442
WD40andTape 4:303584310071 443 //threadSimulateDemand.start(SimulateDemand);
WD40andTape 4:303584310071 444 threadReceiveAndReplan.start(ReceiveAndReplan);//start Replanning thread
WD40andTape 4:303584310071 445 //ReceiveAndReplan();
WD40andTape 4:303584310071 446 threadSmoothPathPlan.start(CalculateSmoothPath); //start planning thread
WD40andTape 1:2a43cf183a62 447
dofydoink 0:607bc887b6e0 448 PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); //set up planning thread to recur at fixed intervals
WD40andTape 1:2a43cf183a62 449
dofydoink 0:607bc887b6e0 450 Thread::wait(1);
WD40andTape 3:c83291bf9fd2 451
dofydoink 0:607bc887b6e0 452
WD40andTape 1:2a43cf183a62 453 while(1) {
WD40andTape 1:2a43cf183a62 454 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 455 }
dofydoink 0:607bc887b6e0 456 }
dofydoink 0:607bc887b6e0 457