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@9:cd3607ba5643, 2018-08-07 (annotated)

Committer:
WD40andTape
Date:
Tue Aug 07 14:15:37 2018 +0000
Revision:
9:cd3607ba5643
Parent:
8:d6657767a182
Child:
10:1b6daba32452
Moved more code into the sub-classes.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WD40andTape 7:5b6a2cefbf3b 1 // STANDARD IMPORTS
WD40andTape 7:5b6a2cefbf3b 2 #include "math.h"
WD40andTape 7:5b6a2cefbf3b 3 // MBED IMPORTS
dofydoink 0:607bc887b6e0 4 #include "mbed.h"
WD40andTape 6:f0a18e28a322 5 #include "mbed_events.h"
WD40andTape 7:5b6a2cefbf3b 6 // CUSTOM IMPORTS
WD40andTape 7:5b6a2cefbf3b 7 #include "HLComms.h"
WD40andTape 8:d6657767a182 8 #include "LLComms.h"
dofydoink 0:607bc887b6e0 9
WD40andTape 7:5b6a2cefbf3b 10 // SIMPLE CHANNEL SELECTION
dofydoink 0:607bc887b6e0 11 #define ADC_PRESSURE 1
dofydoink 0:607bc887b6e0 12 #define ADC_POSITION 3
WD40andTape 7:5b6a2cefbf3b 13
WD40andTape 7:5b6a2cefbf3b 14 #define N_CHANNELS 8 // Number of channels to control
WD40andTape 7:5b6a2cefbf3b 15 // 1-3: front segment; 4-6: rear segment; 7-8: mid segment
dofydoink 0:607bc887b6e0 16
WD40andTape 7:5b6a2cefbf3b 17 // SPI SETTINGS
dofydoink 0:607bc887b6e0 18 #define LOW_LEVEL_SPI_FREQUENCY 10000000
WD40andTape 7:5b6a2cefbf3b 19 // PATH GENERATION SETTINGS
WD40andTape 6:f0a18e28a322 20 #define PATH_SAMPLE_TIME_S 0.005 //0.109
dofydoink 0:607bc887b6e0 21 #define MAX_LENGTH_MM 100.0
WD40andTape 3:c83291bf9fd2 22 #define MAX_ACTUATOR_LENGTH 52.2
dofydoink 0:607bc887b6e0 23 #define MAX_SPEED_MMPS 24.3457
WD40andTape 7:5b6a2cefbf3b 24 #define PATH_TOLERANCE_MM 0.2 // How close the linear path must get to the target position before considering it a success.
WD40andTape 7:5b6a2cefbf3b 25
WD40andTape 7:5b6a2cefbf3b 26 #define BAUD_RATE 9600 //115200
WD40andTape 7:5b6a2cefbf3b 27
WD40andTape 7:5b6a2cefbf3b 28 // COMMS SETTINGS
WD40andTape 7:5b6a2cefbf3b 29 const short int SERVER_PORT = 80;
dofydoink 0:607bc887b6e0 30
WD40andTape 6:f0a18e28a322 31 //const double DBL_MAX_CHAMBER_LENGTHS_MM[N_CHANNELS] = {80.0,80.0,80.0,80.0,80.0,80.0,80.0,80.0};
WD40andTape 7:5b6a2cefbf3b 32 //const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {0.30586,0.30586,0.30586,0.30586,0.30586,0.4588,0.4588}; // Convert from chamber lengths to actuator lengths
WD40andTape 7:5b6a2cefbf3b 33 const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; // Convert from chamber lengths to actuator
WD40andTape 4:303584310071 34 const double DBL_SMOOTHING_FACTOR = 0.5; // 0<x<=1, where 1 is no smoothing
WD40andTape 7:5b6a2cefbf3b 35 const double DBL_PATH_TOLERANCE = MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; // Path tolerance in mm with 5% tolerance
dofydoink 0:607bc887b6e0 36
WD40andTape 7:5b6a2cefbf3b 37 // PATH VARIABLES
WD40andTape 7:5b6a2cefbf3b 38 double dblVelocity_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 39 double dblLinearPathCurrentPos_mm[N_CHANNELS]={ 0.0 }; // The current position of the linear path (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 40 double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
dofydoink 0:607bc887b6e0 41
WD40andTape 7:5b6a2cefbf3b 42 int intDemandPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator
dofydoink 0:607bc887b6e0 43
WD40andTape 7:5b6a2cefbf3b 44 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
WD40andTape 8:d6657767a182 45 LLComms llcomms(LOW_LEVEL_SPI_FREQUENCY);//(N_CHANNELS);
WD40andTape 3:c83291bf9fd2 46
dofydoink 0:607bc887b6e0 47 EventQueue queue(32 * EVENTS_EVENT_SIZE);
dofydoink 0:607bc887b6e0 48 Thread t(osPriorityRealtime);
WD40andTape 4:303584310071 49 Thread threadReceiveAndReplan(osPriorityBelowNormal);
WD40andTape 4:303584310071 50 Thread threadSmoothPathPlan(osPriorityNormal);
dofydoink 0:607bc887b6e0 51
WD40andTape 4:303584310071 52 Mutex mutPathIn;
WD40andTape 7:5b6a2cefbf3b 53 Semaphore semPathPlan(1);
dofydoink 0:607bc887b6e0 54
WD40andTape 7:5b6a2cefbf3b 55 Timer timer;
WD40andTape 7:5b6a2cefbf3b 56 Ticker PathCalculationTicker;
dofydoink 0:607bc887b6e0 57
WD40andTape 9:cd3607ba5643 58 bool isDataReady[N_CHANNELS] = { 0 }; // flag to indicate path data is ready for transmission to low level.
dofydoink 0:607bc887b6e0 59
WD40andTape 7:5b6a2cefbf3b 60 void startPathPlan() { // Plan a new linear path after receiving new target data
WD40andTape 4:303584310071 61 semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
dofydoink 0:607bc887b6e0 62 }
dofydoink 0:607bc887b6e0 63
WD40andTape 7:5b6a2cefbf3b 64 // This function will be called when a new transmission is received from high level
WD40andTape 7:5b6a2cefbf3b 65 void ReceiveAndReplan() {
WD40andTape 7:5b6a2cefbf3b 66 HLComms hlcomms(SERVER_PORT);
WD40andTape 6:f0a18e28a322 67
WD40andTape 6:f0a18e28a322 68 int error_code;
WD40andTape 7:5b6a2cefbf3b 69 error_code = hlcomms.setup_server();
WD40andTape 6:f0a18e28a322 70 if( error_code == -1 ) return;
WD40andTape 7:5b6a2cefbf3b 71 error_code = hlcomms.accept_connection();
WD40andTape 7:5b6a2cefbf3b 72 if( error_code == -1 ) {
WD40andTape 7:5b6a2cefbf3b 73 hlcomms.close_server();
WD40andTape 7:5b6a2cefbf3b 74 return;
WD40andTape 7:5b6a2cefbf3b 75 }
WD40andTape 6:f0a18e28a322 76
WD40andTape 7:5b6a2cefbf3b 77 int ii;
WD40andTape 7:5b6a2cefbf3b 78 struct msg_format input; //hlcomms.msg_format
WD40andTape 4:303584310071 79
WD40andTape 6:f0a18e28a322 80 while( true ) {
WD40andTape 6:f0a18e28a322 81 // RECEIVE MESSAGE
WD40andTape 9:cd3607ba5643 82 error_code = hlcomms.receive_message();
WD40andTape 6:f0a18e28a322 83 if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
WD40andTape 6:f0a18e28a322 84 printf("Client disconnected.\n\r");
WD40andTape 7:5b6a2cefbf3b 85 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 86 return;
WD40andTape 6:f0a18e28a322 87 } else if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 88 printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 89 "Perhaps the server socket is not connected to a remote host? "
WD40andTape 6:f0a18e28a322 90 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 91 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 92 return;
WD40andTape 6:f0a18e28a322 93 }
WD40andTape 6:f0a18e28a322 94 //printf("Message received.\r\n");
WD40andTape 9:cd3607ba5643 95 input = hlcomms.process_message();
WD40andTape 1:2a43cf183a62 96
WD40andTape 7:5b6a2cefbf3b 97 // PROCESS INPUT
WD40andTape 7:5b6a2cefbf3b 98 double dblTargetChambLen_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
WD40andTape 6:f0a18e28a322 99 printf("REPLAN, %f\r\n",input.duration);
WD40andTape 7:5b6a2cefbf3b 100 // Update front segment
WD40andTape 6:f0a18e28a322 101 dblTargetChambLen_mm[0] = input.psi[0][0]*1000;
WD40andTape 6:f0a18e28a322 102 dblTargetChambLen_mm[1] = input.psi[0][1]*1000;
WD40andTape 6:f0a18e28a322 103 dblTargetChambLen_mm[2] = input.psi[0][2]*1000;
WD40andTape 7:5b6a2cefbf3b 104 // Update mid segment
WD40andTape 6:f0a18e28a322 105 dblTargetChambLen_mm[6] = input.psi[1][0]*1000;
WD40andTape 7:5b6a2cefbf3b 106 dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; // Same because two pumps are used
WD40andTape 7:5b6a2cefbf3b 107 // Update rear segment
WD40andTape 6:f0a18e28a322 108 dblTargetChambLen_mm[3] = input.psi[2][0]*1000;
WD40andTape 6:f0a18e28a322 109 dblTargetChambLen_mm[4] = input.psi[2][1]*1000;
WD40andTape 6:f0a18e28a322 110 dblTargetChambLen_mm[5] = input.psi[2][2]*1000;
dofydoink 0:607bc887b6e0 111
WD40andTape 6:f0a18e28a322 112 bool isTimeChanged = 0;
WD40andTape 6:f0a18e28a322 113 double dblMaxRecalculatedTime = input.duration;
WD40andTape 7:5b6a2cefbf3b 114 mutPathIn.lock(); // Lock variables to avoid race condition
WD40andTape 4:303584310071 115 for (ii = 0; ii< N_CHANNELS; ii++) {
dofydoink 0:607bc887b6e0 116 //check to see if positions are achievable
WD40andTape 6:f0a18e28a322 117 /*if(dblTargetChambLen_mm[ii]>DBL_MAX_CHAMBER_LENGTHS_MM[ii]) {
WD40andTape 4:303584310071 118 dblTargetChambLen_mm[ii] = DBL_MAX_CHAMBER_LENGTHS_MM[ii];
WD40andTape 6:f0a18e28a322 119 isTimeChanged = 1;
dofydoink 0:607bc887b6e0 120 }
WD40andTape 4:303584310071 121 if(dblTargetChambLen_mm[ii]<0.0) {
WD40andTape 6:f0a18e28a322 122 dblTargetChambLen_mm[ii] = 0.0;
WD40andTape 6:f0a18e28a322 123 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 124 }*/
WD40andTape 6:f0a18e28a322 125 dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
WD40andTape 6:f0a18e28a322 126 //!! LIMIT CHAMBER LENGTHS TOO
WD40andTape 6:f0a18e28a322 127 if( dblTargetActPos_mm[ii]<0.0 || dblTargetActPos_mm[ii]>25.0 ) {
WD40andTape 6:f0a18e28a322 128 dblTargetActPos_mm[ii] = min( max( 0.0 , dblTargetActPos_mm[ii] ) , 25.0 );
WD40andTape 6:f0a18e28a322 129 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 130 }
WD40andTape 6:f0a18e28a322 131 }
WD40andTape 6:f0a18e28a322 132 double dblActPosChange;
WD40andTape 6:f0a18e28a322 133 short sgn;
WD40andTape 7:5b6a2cefbf3b 134 for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
WD40andTape 6:f0a18e28a322 135 dblActPosChange = dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii];
WD40andTape 6:f0a18e28a322 136 if( fabs(dblActPosChange) < DBL_PATH_TOLERANCE ) {
WD40andTape 6:f0a18e28a322 137 dblActPosChange = 0.0;
WD40andTape 6:f0a18e28a322 138 isTimeChanged = 1;
dofydoink 0:607bc887b6e0 139 }
WD40andTape 6:f0a18e28a322 140 if( input.duration < 0.000000001 ) {
WD40andTape 6:f0a18e28a322 141 sgn = (dblActPosChange > 0) ? 1 : ((dblActPosChange < 0) ? -1 : 0);
WD40andTape 6:f0a18e28a322 142 dblVelocity_mmps[ii] = sgn * MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 143 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 144 } else {
WD40andTape 6:f0a18e28a322 145 dblVelocity_mmps[ii] = dblActPosChange / input.duration;
WD40andTape 6:f0a18e28a322 146 }
WD40andTape 7:5b6a2cefbf3b 147 // Check to see if velocities are achievable
WD40andTape 6:f0a18e28a322 148 if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
WD40andTape 6:f0a18e28a322 149 if(dblVelocity_mmps[ii]>0) {
WD40andTape 6:f0a18e28a322 150 dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 151 } else {
WD40andTape 6:f0a18e28a322 152 dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 153 }
WD40andTape 6:f0a18e28a322 154 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 155 }
WD40andTape 6:f0a18e28a322 156 double dblRecalculatedTime;
WD40andTape 6:f0a18e28a322 157 if( fabs(dblVelocity_mmps[ii]) < 0.000000001 ) {
WD40andTape 6:f0a18e28a322 158 dblRecalculatedTime = 0;
WD40andTape 6:f0a18e28a322 159 } else {
WD40andTape 6:f0a18e28a322 160 dblRecalculatedTime = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblVelocity_mmps[ii];
WD40andTape 6:f0a18e28a322 161 }
WD40andTape 6:f0a18e28a322 162 if( dblRecalculatedTime > dblMaxRecalculatedTime ) {
WD40andTape 6:f0a18e28a322 163 dblMaxRecalculatedTime = dblRecalculatedTime;
WD40andTape 6:f0a18e28a322 164 }
WD40andTape 6:f0a18e28a322 165 }
WD40andTape 6:f0a18e28a322 166 if( isTimeChanged ) {
WD40andTape 6:f0a18e28a322 167 if( dblMaxRecalculatedTime < input.duration ) {
WD40andTape 6:f0a18e28a322 168 dblMaxRecalculatedTime = input.duration;
WD40andTape 6:f0a18e28a322 169 }
WD40andTape 7:5b6a2cefbf3b 170 for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
WD40andTape 6:f0a18e28a322 171 dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblMaxRecalculatedTime;
WD40andTape 6:f0a18e28a322 172 }
WD40andTape 6:f0a18e28a322 173 }
WD40andTape 7:5b6a2cefbf3b 174 mutPathIn.unlock(); // Unlock mutex
WD40andTape 6:f0a18e28a322 175
WD40andTape 6:f0a18e28a322 176 printf("Sending message...\r\n");
WD40andTape 6:f0a18e28a322 177 // SEND MESSAGE
WD40andTape 9:cd3607ba5643 178 hlcomms.make_message(&dblMaxRecalculatedTime);
WD40andTape 9:cd3607ba5643 179 error_code = hlcomms.send_message();
WD40andTape 6:f0a18e28a322 180 if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 181 printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 182 "Perhaps the server socket is not bound or not set to listen for connections? "
WD40andTape 6:f0a18e28a322 183 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 184 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 185 return;
WD40andTape 6:f0a18e28a322 186 }
WD40andTape 6:f0a18e28a322 187 printf("Message sent.\r\n");
WD40andTape 6:f0a18e28a322 188 }
WD40andTape 6:f0a18e28a322 189
dofydoink 0:607bc887b6e0 190 }
dofydoink 0:607bc887b6e0 191
WD40andTape 4:303584310071 192 void CalculateSmoothPath() {
dofydoink 5:712e7634c779 193 int jj;
WD40andTape 1:2a43cf183a62 194 double dblMeasuredSampleTime;
WD40andTape 7:5b6a2cefbf3b 195 double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; // The current position of the smooth path (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 196 //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 7:5b6a2cefbf3b 197 //double dblPressure_bar[N_CHANNELS]; // The pressure in a given chamber in bar (1 bar = 100,000 Pa)
WD40andTape 4:303584310071 198 while(1) {
dofydoink 0:607bc887b6e0 199 semPathPlan.wait();
WD40andTape 1:2a43cf183a62 200
WD40andTape 1:2a43cf183a62 201 // If run time is more than 50 us from expected, calculate from measured time step
WD40andTape 4:303584310071 202 if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50) {
dofydoink 5:712e7634c779 203 dblMeasuredSampleTime = timer.read();
WD40andTape 4:303584310071 204 } else {
WD40andTape 1:2a43cf183a62 205 dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
WD40andTape 1:2a43cf183a62 206 }
WD40andTape 4:303584310071 207 timer.reset();
WD40andTape 1:2a43cf183a62 208
dofydoink 5:712e7634c779 209 for(jj = 0; jj < N_CHANNELS; jj++) {
WD40andTape 7:5b6a2cefbf3b 210 //dblPressure_bar[jj] = ReadADCPressure_bar(jj); // Read pressure from channel
WD40andTape 7:5b6a2cefbf3b 211 //dblPosition_mtrs[jj] = ReadADCPosition_mtrs(jj); // Read position from channel
WD40andTape 1:2a43cf183a62 212
WD40andTape 7:5b6a2cefbf3b 213 // Calculate next step in linear path
WD40andTape 7:5b6a2cefbf3b 214 mutPathIn.lock(); // Lock relevant mutex
WD40andTape 7:5b6a2cefbf3b 215 dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime;
WD40andTape 7:5b6a2cefbf3b 216 if(dblLinearPathCurrentPos_mm[jj] < 0.0) {
WD40andTape 7:5b6a2cefbf3b 217 dblLinearPathCurrentPos_mm[jj] = 0.00;
WD40andTape 7:5b6a2cefbf3b 218 }
WD40andTape 7:5b6a2cefbf3b 219 // Check tolerance
WD40andTape 7:5b6a2cefbf3b 220 if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= DBL_PATH_TOLERANCE) {
WD40andTape 7:5b6a2cefbf3b 221 dblVelocity_mmps[jj] = 0.0; // Stop linear path generation when linear path is within tolerance of target position
WD40andTape 7:5b6a2cefbf3b 222 }
WD40andTape 7:5b6a2cefbf3b 223 mutPathIn.unlock(); // Unlock relevant mutex
dofydoink 0:607bc887b6e0 224
WD40andTape 7:5b6a2cefbf3b 225 // Calculate next step in smooth path
dofydoink 5:712e7634c779 226 dblSmoothPathCurrentPos_mm[jj] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
WD40andTape 8:d6657767a182 227 llcomms.mutChannel[jj].lock(); // MUTEX LOCK
WD40andTape 7:5b6a2cefbf3b 228 intDemandPos_Tx[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);// Convert to a 13-bit number
WD40andTape 7:5b6a2cefbf3b 229 intDemandPos_Tx[jj] = intDemandPos_Tx[jj] & 0x1FFF; // Ensure number is 13-bit
WD40andTape 8:d6657767a182 230 llcomms.mutChannel[jj].unlock(); // MUTEX UNLOCK
dofydoink 0:607bc887b6e0 231
dofydoink 5:712e7634c779 232 isDataReady[jj] = 1;//signal that data ready
WD40andTape 4:303584310071 233 } // end for
WD40andTape 7:5b6a2cefbf3b 234
WD40andTape 6:f0a18e28a322 235 //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
WD40andTape 6:f0a18e28a322 236 /*printf("%f, %f, %f, %f, %f, %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblLinearPathCurrentPos_mm[1],dblLinearPathCurrentPos_mm[2],
WD40andTape 6:f0a18e28a322 237 dblLinearPathCurrentPos_mm[3],dblLinearPathCurrentPos_mm[4],dblLinearPathCurrentPos_mm[5],dblLinearPathCurrentPos_mm[6],dblLinearPathCurrentPos_mm[7]);*/
WD40andTape 6:f0a18e28a322 238 //printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
WD40andTape 7:5b6a2cefbf3b 239
WD40andTape 4:303584310071 240 } // end while
WD40andTape 3:c83291bf9fd2 241 }
dofydoink 0:607bc887b6e0 242
WD40andTape 8:d6657767a182 243 // NEED TO FIGURE OUT POINTER-TO-MEMBER FUNCTIONS TO MOVE THESE INTO LLComms CLASS
WD40andTape 8:d6657767a182 244 void SendReceiveData(int channel, int _intDemandPos_Tx[], bool (*_isDataReady)[8]) {
WD40andTape 8:d6657767a182 245 int intPosSPI_Rx[N_CHANNELS]; // 13 bit value received over SPI from the actuator
WD40andTape 8:d6657767a182 246
WD40andTape 8:d6657767a182 247 // Get data from controller
WD40andTape 8:d6657767a182 248 llcomms.spi.format(16,2);
WD40andTape 8:d6657767a182 249 llcomms.spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 8:d6657767a182 250 llcomms.mutChannel[channel].lock(); // Lock mutex for specific Channel
WD40andTape 8:d6657767a182 251 *llcomms.cs_LL[channel] = 0; // Select relevant chip
WD40andTape 8:d6657767a182 252 intPosSPI_Rx[channel] = llcomms.spi.write(_intDemandPos_Tx[channel]); // Transmit & receive
WD40andTape 8:d6657767a182 253 *llcomms.cs_LL[channel] = 1; // Deselect chip
WD40andTape 8:d6657767a182 254 *_isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
WD40andTape 8:d6657767a182 255 /*if(channel == 0) {
WD40andTape 8:d6657767a182 256 intGlobalTest = intPosSPI_Rx[channel];
WD40andTape 8:d6657767a182 257 dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
WD40andTape 8:d6657767a182 258 }*/
WD40andTape 8:d6657767a182 259
WD40andTape 8:d6657767a182 260 // Sort out received data
WD40andTape 8:d6657767a182 261 llcomms.chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
WD40andTape 8:d6657767a182 262
WD40andTape 8:d6657767a182 263 intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
WD40andTape 8:d6657767a182 264 //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
WD40andTape 8:d6657767a182 265 llcomms.mutChannel[channel].unlock();//unlock mutex for specific channel
WD40andTape 8:d6657767a182 266 //printf("%d, %d\r\n",intPosSPI_Rx[0],_intDemandPos_Tx[0]);
WD40andTape 8:d6657767a182 267 }
WD40andTape 8:d6657767a182 268 // Common rise handler function
WD40andTape 8:d6657767a182 269 void common_rise_handler(int channel) {
WD40andTape 8:d6657767a182 270 llcomms.pinCheck = 1;
WD40andTape 8:d6657767a182 271 if (isDataReady[channel]) { // Check if data is ready for tranmission
WD40andTape 8:d6657767a182 272 //llcomms.ThreadID[channel] = queue.call(&llcomms.SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 8:d6657767a182 273 //llcomms.ThreadID[channel] = queue.call(&llcomms.SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 8:d6657767a182 274 //llcomms.ThreadID[channel] = queue.call(mbed::Callback(&llcomms,&LLComms::SendReceiveData),channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 8:d6657767a182 275 // llcomms.ThreadID[channel] = queue.call(mbed::Callback<void()>(&llcomms,&LLComms::SendReceiveData),channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 8:d6657767a182 276 //llcomms.ThreadID[channel] = queue.call(&llcomms,*llcomms.SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 8:d6657767a182 277 llcomms.ThreadID[channel] = queue.call(&SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 9:cd3607ba5643 278 //llcomms.ThreadID[channel] = queue.call(&llcomms,&LLComms::dSendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 8:d6657767a182 279 //callback(&low_pass1, &LowPass::step)
WD40andTape 8:d6657767a182 280 }
WD40andTape 8:d6657767a182 281 }
WD40andTape 8:d6657767a182 282 // Common fall handler functions
WD40andTape 8:d6657767a182 283 void common_fall_handler(int channel) {
WD40andTape 8:d6657767a182 284 llcomms.pinCheck = 0;
WD40andTape 8:d6657767a182 285 queue.cancel(llcomms.ThreadID[channel]); // Cancel relevant queued event
WD40andTape 8:d6657767a182 286 }
WD40andTape 8:d6657767a182 287 // Stub rise functions
WD40andTape 8:d6657767a182 288 void rise0(void) { common_rise_handler(0); }
WD40andTape 8:d6657767a182 289 void rise1(void) { common_rise_handler(1); }
WD40andTape 8:d6657767a182 290 void rise2(void) { common_rise_handler(2); }
WD40andTape 8:d6657767a182 291 void rise3(void) { common_rise_handler(3); }
WD40andTape 8:d6657767a182 292 void rise4(void) { common_rise_handler(4); }
WD40andTape 8:d6657767a182 293 void rise5(void) { common_rise_handler(5); }
WD40andTape 8:d6657767a182 294 void rise6(void) { common_rise_handler(6); }
WD40andTape 8:d6657767a182 295 void rise7(void) { common_rise_handler(7); }
WD40andTape 8:d6657767a182 296 // Stub fall functions
WD40andTape 8:d6657767a182 297 void fall0(void) { common_fall_handler(0); }
WD40andTape 8:d6657767a182 298 void fall1(void) { common_fall_handler(1); }
WD40andTape 8:d6657767a182 299 void fall2(void) { common_fall_handler(2); }
WD40andTape 8:d6657767a182 300 void fall3(void) { common_fall_handler(3); }
WD40andTape 8:d6657767a182 301 void fall4(void) { common_fall_handler(4); }
WD40andTape 8:d6657767a182 302 void fall5(void) { common_fall_handler(5); }
WD40andTape 8:d6657767a182 303 void fall6(void) { common_fall_handler(6); }
WD40andTape 8:d6657767a182 304 void fall7(void) { common_fall_handler(7); }
WD40andTape 8:d6657767a182 305
WD40andTape 9:cd3607ba5643 306 int main() {
WD40andTape 7:5b6a2cefbf3b 307 pc.baud(BAUD_RATE);
dofydoink 0:607bc887b6e0 308 printf("Hi, there! I'll be your mid-level controller for today.\r\n");
dofydoink 5:712e7634c779 309 wait(3);
WD40andTape 9:cd3607ba5643 310
WD40andTape 9:cd3607ba5643 311 llcomms.reset();
WD40andTape 7:5b6a2cefbf3b 312 t.start(callback(&queue, &EventQueue::dispatch_forever)); // Start the event queue
dofydoink 0:607bc887b6e0 313
WD40andTape 7:5b6a2cefbf3b 314 // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 8:d6657767a182 315 llcomms.pinGate0.rise(&rise0);
WD40andTape 8:d6657767a182 316 llcomms.pinGate1.rise(&rise1);
WD40andTape 8:d6657767a182 317 llcomms.pinGate2.rise(&rise2);
WD40andTape 8:d6657767a182 318 llcomms.pinGate3.rise(&rise3);
WD40andTape 8:d6657767a182 319 llcomms.pinGate4.rise(&rise4);
WD40andTape 8:d6657767a182 320 llcomms.pinGate5.rise(&rise5);
WD40andTape 8:d6657767a182 321 llcomms.pinGate6.rise(&rise6);
WD40andTape 8:d6657767a182 322 llcomms.pinGate7.rise(&rise7);
WD40andTape 7:5b6a2cefbf3b 323 // Set up fall interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 8:d6657767a182 324 llcomms.pinGate0.fall(&fall0);
WD40andTape 9:cd3607ba5643 325 //llcomms.pinGate0.fall(&llcomms,&LLComms::dfall0);
WD40andTape 8:d6657767a182 326 llcomms.pinGate1.fall(&fall1);
WD40andTape 8:d6657767a182 327 llcomms.pinGate2.fall(&fall2);
WD40andTape 8:d6657767a182 328 llcomms.pinGate3.fall(&fall3);
WD40andTape 8:d6657767a182 329 llcomms.pinGate4.fall(&fall4);
WD40andTape 8:d6657767a182 330 llcomms.pinGate5.fall(&fall5);
WD40andTape 8:d6657767a182 331 llcomms.pinGate6.fall(&fall6);
WD40andTape 8:d6657767a182 332 llcomms.pinGate7.fall(&fall7);
dofydoink 5:712e7634c779 333
dofydoink 0:607bc887b6e0 334 timer.start();
WD40andTape 7:5b6a2cefbf3b 335
WD40andTape 7:5b6a2cefbf3b 336 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
WD40andTape 7:5b6a2cefbf3b 337 threadSmoothPathPlan.start(CalculateSmoothPath); // Start planning thread
WD40andTape 7:5b6a2cefbf3b 338
WD40andTape 7:5b6a2cefbf3b 339 PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); // Set up planning thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 340
WD40andTape 7:5b6a2cefbf3b 341 Thread::wait(1);
WD40andTape 1:2a43cf183a62 342 while(1) {
WD40andTape 1:2a43cf183a62 343 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 344 }
WD40andTape 7:5b6a2cefbf3b 345 }