MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

main.cpp@2:eea12b149dba, 2018-07-31 (annotated)

Committer:
WD40andTape
Date:
Tue Jul 31 10:44:10 2018 +0000
Revision:
2:eea12b149dba
Parent:
1:2a43cf183a62
Child:
3:c83291bf9fd2
Changes prior to incorporating HL/ML comms.

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