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@16:1e2804a4e5bd, 2018-09-11 (annotated)

Committer:
WD40andTape
Date:
Tue Sep 11 10:10:26 2018 +0000
Revision:
16:1e2804a4e5bd
Parent:
15:59471daef4cb
Child:
17:bbaf3e8440ad
TEMP: Sending position and pressure for one actuator over serial.

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
dofydoink 12:595ed862e52f 7 #include "MLSettings.h"
WD40andTape 7:5b6a2cefbf3b 8 #include "HLComms.h"
WD40andTape 8:d6657767a182 9 #include "LLComms.h"
dofydoink 0:607bc887b6e0 10
WD40andTape 13:a373dfc57b89 11 //DigitalOut pinTesty(PB_8);
WD40andTape 13:a373dfc57b89 12
dofydoink 12:595ed862e52f 13 // Maximum achievable mm path tolerance plus additional % tolerance
dofydoink 12:595ed862e52f 14 const float FLT_PATH_TOLERANCE = MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * (1.0f+FLT_PERCENT_PATH_TOLERANCE);
dofydoink 0:607bc887b6e0 15
WD40andTape 7:5b6a2cefbf3b 16 // PATH VARIABLES
WD40andTape 7:5b6a2cefbf3b 17 double dblVelocity_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
WD40andTape 16:1e2804a4e5bd 18 double dblLinearPathCurrentPos_mm[N_CHANNELS]= { 0.0 }; // The current position of the linear path (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 19 double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
dofydoink 0:607bc887b6e0 20
dofydoink 12:595ed862e52f 21 // These have to be declared in the global scope or we get a stack overflow. No idea why.
dofydoink 11:7029367a1840 22 double _dblVelocity_mmps[N_CHANNELS];
dofydoink 12:595ed862e52f 23 //double _dblLinearPathCurrentPos_mm[N_CHANNELS];
dofydoink 12:595ed862e52f 24 double dblADCCurrentPosition[N_CHANNELS];
dofydoink 11:7029367a1840 25
WD40andTape 7:5b6a2cefbf3b 26 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
dofydoink 12:595ed862e52f 27 LLComms llcomms;
WD40andTape 16:1e2804a4e5bd 28 HLComms hlcomms(SERVER_PORT);
WD40andTape 3:c83291bf9fd2 29
WD40andTape 10:1b6daba32452 30 Thread threadLowLevelSPI(osPriorityRealtime);
WD40andTape 13:a373dfc57b89 31 Thread threadSmoothPathPlan(osPriorityNormal);
WD40andTape 4:303584310071 32 Thread threadReceiveAndReplan(osPriorityBelowNormal);
dofydoink 0:607bc887b6e0 33
WD40andTape 4:303584310071 34 Mutex mutPathIn;
WD40andTape 7:5b6a2cefbf3b 35 Semaphore semPathPlan(1);
dofydoink 0:607bc887b6e0 36
WD40andTape 7:5b6a2cefbf3b 37 Timer timer;
WD40andTape 7:5b6a2cefbf3b 38 Ticker PathCalculationTicker;
WD40andTape 16:1e2804a4e5bd 39 Ticker SendPressuresTicker;
dofydoink 0:607bc887b6e0 40
WD40andTape 7:5b6a2cefbf3b 41 void startPathPlan() { // Plan a new linear path after receiving new target data
WD40andTape 4:303584310071 42 semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
dofydoink 0:607bc887b6e0 43 }
dofydoink 0:607bc887b6e0 44
WD40andTape 7:5b6a2cefbf3b 45 // This function will be called when a new transmission is received from high level
WD40andTape 7:5b6a2cefbf3b 46 void ReceiveAndReplan() {
WD40andTape 6:f0a18e28a322 47
WD40andTape 6:f0a18e28a322 48 int error_code;
WD40andTape 7:5b6a2cefbf3b 49 error_code = hlcomms.setup_server();
WD40andTape 6:f0a18e28a322 50 if( error_code == -1 ) return;
WD40andTape 7:5b6a2cefbf3b 51 error_code = hlcomms.accept_connection();
WD40andTape 7:5b6a2cefbf3b 52 if( error_code == -1 ) {
WD40andTape 7:5b6a2cefbf3b 53 hlcomms.close_server();
WD40andTape 7:5b6a2cefbf3b 54 return;
WD40andTape 7:5b6a2cefbf3b 55 }
WD40andTape 6:f0a18e28a322 56
WD40andTape 7:5b6a2cefbf3b 57 int ii;
WD40andTape 7:5b6a2cefbf3b 58 struct msg_format input; //hlcomms.msg_format
WD40andTape 4:303584310071 59
WD40andTape 6:f0a18e28a322 60 while( true ) {
WD40andTape 6:f0a18e28a322 61 // RECEIVE MESSAGE
WD40andTape 9:cd3607ba5643 62 error_code = hlcomms.receive_message();
WD40andTape 6:f0a18e28a322 63 if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
dofydoink 12:595ed862e52f 64 if(IS_PRINT_OUTPUT) printf("Client disconnected.\n\r");
WD40andTape 7:5b6a2cefbf3b 65 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 66 return;
WD40andTape 6:f0a18e28a322 67 } else if( error_code < 0 ) {
dofydoink 12:595ed862e52f 68 if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 69 "Perhaps the server socket is not connected to a remote host? "
WD40andTape 6:f0a18e28a322 70 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 71 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 72 return;
WD40andTape 6:f0a18e28a322 73 }
WD40andTape 9:cd3607ba5643 74 input = hlcomms.process_message();
WD40andTape 1:2a43cf183a62 75
WD40andTape 7:5b6a2cefbf3b 76 // PROCESS INPUT
WD40andTape 7:5b6a2cefbf3b 77 double dblTargetChambLen_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
dofydoink 12:595ed862e52f 78 if(IS_PRINT_OUTPUT) printf("REPLAN, %f\r\n",input.duration);
WD40andTape 7:5b6a2cefbf3b 79 // Update front segment
dofydoink 11:7029367a1840 80 /*dblTargetChambLen_mm[0] = input.psi[0][0]*1000;
WD40andTape 6:f0a18e28a322 81 dblTargetChambLen_mm[1] = input.psi[0][1]*1000;
WD40andTape 6:f0a18e28a322 82 dblTargetChambLen_mm[2] = input.psi[0][2]*1000;
WD40andTape 7:5b6a2cefbf3b 83 // Update mid segment
WD40andTape 6:f0a18e28a322 84 dblTargetChambLen_mm[6] = input.psi[1][0]*1000;
WD40andTape 7:5b6a2cefbf3b 85 dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; // Same because two pumps are used
WD40andTape 7:5b6a2cefbf3b 86 // Update rear segment
WD40andTape 6:f0a18e28a322 87 dblTargetChambLen_mm[3] = input.psi[2][0]*1000;
WD40andTape 6:f0a18e28a322 88 dblTargetChambLen_mm[4] = input.psi[2][1]*1000;
dofydoink 11:7029367a1840 89 dblTargetChambLen_mm[5] = input.psi[2][2]*1000;*/
WD40andTape 14:54c3759e76ed 90
WD40andTape 14:54c3759e76ed 91 //update rear Segment
dofydoink 11:7029367a1840 92 dblTargetChambLen_mm[3] = input.psi[0][0]*1000;
WD40andTape 14:54c3759e76ed 93 dblTargetChambLen_mm[5] = 0.0;//input.psi[0][1]*1000;//not used
WD40andTape 14:54c3759e76ed 94 dblTargetChambLen_mm[6] = 0.0;//input.psi[0][2]*1000;//not used
WD40andTape 14:54c3759e76ed 95
WD40andTape 14:54c3759e76ed 96 //update mid segment
WD40andTape 14:54c3759e76ed 97 dblTargetChambLen_mm[4] = input.psi[1][0]*1000;
WD40andTape 14:54c3759e76ed 98 dblTargetChambLen_mm[7] = dblTargetChambLen_mm[4]; // Same because two pumps are used
WD40andTape 14:54c3759e76ed 99
WD40andTape 14:54c3759e76ed 100 // Update front segment
dofydoink 11:7029367a1840 101 dblTargetChambLen_mm[0] = input.psi[2][0]*1000;
dofydoink 11:7029367a1840 102 dblTargetChambLen_mm[1] = input.psi[2][1]*1000;
dofydoink 11:7029367a1840 103 dblTargetChambLen_mm[2] = input.psi[2][2]*1000;
dofydoink 11:7029367a1840 104
dofydoink 11:7029367a1840 105 mutPathIn.lock(); // Lock variables to avoid race condition
dofydoink 12:595ed862e52f 106 /*for(int j = 0; j<N_CHANNELS; j++) {
dofydoink 11:7029367a1840 107 //_dblVelocity_mmps[j] = dblVelocity_mmps[j];
dofydoink 11:7029367a1840 108 _dblLinearPathCurrentPos_mm[j] = dblLinearPathCurrentPos_mm[j];
dofydoink 12:595ed862e52f 109 }*/
dofydoink 12:595ed862e52f 110 for(int j = 0; j<N_CHANNELS; j++) {
dofydoink 12:595ed862e52f 111 dblADCCurrentPosition[j] = dblLinearPathCurrentPos_mm[j];
dofydoink 12:595ed862e52f 112 //dblADCCurrentPosition[j] = llcomms.ReadADCPosition_mtrs(j); // Read position from channel
dofydoink 11:7029367a1840 113 }
dofydoink 12:595ed862e52f 114
dofydoink 11:7029367a1840 115 mutPathIn.unlock(); // Unlock mutex
dofydoink 0:607bc887b6e0 116
WD40andTape 13:a373dfc57b89 117 //bool isTimeChanged = 0;
WD40andTape 6:f0a18e28a322 118 double dblMaxRecalculatedTime = input.duration;
WD40andTape 13:a373dfc57b89 119 // Convert from requested chamber to actuator space and limit actuator positions
WD40andTape 4:303584310071 120 for (ii = 0; ii< N_CHANNELS; ii++) {
WD40andTape 13:a373dfc57b89 121 // If sent a negative requested position, do NOT replan that actuator
WD40andTape 14:54c3759e76ed 122 if( dblTargetChambLen_mm[ii] < 0.0 ) continue;
dofydoink 0:607bc887b6e0 123 //check to see if positions are achievable
dofydoink 12:595ed862e52f 124 dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*FLT_ACTUATOR_CONVERSION[ii];
WD40andTape 15:59471daef4cb 125 dblTargetActPos_mm[ii] = min( max( 0.0 , dblTargetActPos_mm[ii] ) , 40.0 );
WD40andTape 6:f0a18e28a322 126 //!! LIMIT CHAMBER LENGTHS TOO
WD40andTape 6:f0a18e28a322 127 }
WD40andTape 13:a373dfc57b89 128 // Calculate achievable velocities, and hence time, for the requested move to complete within tolerance
WD40andTape 6:f0a18e28a322 129 double dblActPosChange;
WD40andTape 6:f0a18e28a322 130 short sgn;
WD40andTape 7:5b6a2cefbf3b 131 for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
WD40andTape 13:a373dfc57b89 132 // If sent a negative requested position, do NOT replan that actuator
WD40andTape 14:54c3759e76ed 133 if( dblTargetChambLen_mm[ii] < 0.0 ) continue;
WD40andTape 13:a373dfc57b89 134
dofydoink 11:7029367a1840 135 /*dblActPosChange = 1.0; // or = 0.0;
dofydoink 11:7029367a1840 136 _dblVelocity_mmps[ii] = 0.0;*/ // DOESN'T CRASH
dofydoink 11:7029367a1840 137 /*dblActPosChange = dblTargetActPos_mm[ii];
dofydoink 11:7029367a1840 138 _dblVelocity_mmps[ii] = 0.0;*/ // DOESN'T CRASH
dofydoink 11:7029367a1840 139 /*dblActPosChange = _dblLinearPathCurrentPos_mm[ii];*/ // DOESN'T CRASH
dofydoink 11:7029367a1840 140 /*_dblVelocity_mmps[ii] = _dblLinearPathCurrentPos_mm[ii];*/ // DOESN'T CRASH
dofydoink 11:7029367a1840 141 /*dblActPosChange = 0.0;
dofydoink 11:7029367a1840 142 _dblVelocity_mmps[ii] = _dblLinearPathCurrentPos_mm[ii];*/ // DOESN'T CRASH
dofydoink 11:7029367a1840 143
dofydoink 11:7029367a1840 144 // DOES CRASH but not with a return at the end of while OR if _ variables are declared globally
dofydoink 11:7029367a1840 145 /*dblActPosChange = _dblLinearPathCurrentPos_mm[ii];
dofydoink 11:7029367a1840 146 _dblVelocity_mmps[ii] = 0.0;*/
dofydoink 11:7029367a1840 147
dofydoink 12:595ed862e52f 148 dblActPosChange = dblTargetActPos_mm[ii] - dblADCCurrentPosition[ii];
WD40andTape 13:a373dfc57b89 149 if( fabs(dblActPosChange) < FLT_PATH_TOLERANCE ) { // If actuator ii is already within tolerance
WD40andTape 6:f0a18e28a322 150 dblActPosChange = 0.0;
WD40andTape 13:a373dfc57b89 151 //isTimeChanged = 1;
dofydoink 0:607bc887b6e0 152 }
dofydoink 11:7029367a1840 153 //IS BELOW
WD40andTape 13:a373dfc57b89 154 if( input.duration < 0.000000001 ) { // If max (safe) velocity was requested
WD40andTape 6:f0a18e28a322 155 sgn = (dblActPosChange > 0) ? 1 : ((dblActPosChange < 0) ? -1 : 0);
dofydoink 11:7029367a1840 156 _dblVelocity_mmps[ii] = sgn * MAX_SPEED_MMPS;
WD40andTape 13:a373dfc57b89 157 //isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 158 } else {
dofydoink 11:7029367a1840 159 _dblVelocity_mmps[ii] = dblActPosChange / input.duration;
WD40andTape 6:f0a18e28a322 160 }
dofydoink 11:7029367a1840 161 //IS ABOVE
WD40andTape 13:a373dfc57b89 162 // Check to see if velocities are achievable -- this can move into the else section of the above if statement
dofydoink 11:7029367a1840 163 if(abs(_dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
dofydoink 11:7029367a1840 164 if(_dblVelocity_mmps[ii]>0) {
dofydoink 11:7029367a1840 165 _dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 166 } else {
dofydoink 11:7029367a1840 167 _dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 168 }
WD40andTape 13:a373dfc57b89 169 //isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 170 }
WD40andTape 13:a373dfc57b89 171 // Recalculate the move's time after altering the position and/or velocity
WD40andTape 6:f0a18e28a322 172 double dblRecalculatedTime;
dofydoink 11:7029367a1840 173 if( fabs(_dblVelocity_mmps[ii]) < 0.000000001 ) {
WD40andTape 6:f0a18e28a322 174 dblRecalculatedTime = 0;
WD40andTape 6:f0a18e28a322 175 } else {
dofydoink 11:7029367a1840 176 dblRecalculatedTime = dblActPosChange / _dblVelocity_mmps[ii];
WD40andTape 6:f0a18e28a322 177 }
WD40andTape 13:a373dfc57b89 178 // Find the maximum time for any actuator's move for synchronization
WD40andTape 6:f0a18e28a322 179 if( dblRecalculatedTime > dblMaxRecalculatedTime ) {
WD40andTape 6:f0a18e28a322 180 dblMaxRecalculatedTime = dblRecalculatedTime;
WD40andTape 6:f0a18e28a322 181 }
WD40andTape 6:f0a18e28a322 182 }
WD40andTape 13:a373dfc57b89 183 // Finally recalculate all of the velocities based upon this maximum time for synchronization
WD40andTape 13:a373dfc57b89 184 // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!MUTEX FOR dblVelocity_mmps and dblTargetActPos_mm??
WD40andTape 13:a373dfc57b89 185 for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
WD40andTape 13:a373dfc57b89 186 // If sent a negative requested position, do NOT replan that actuator
WD40andTape 14:54c3759e76ed 187 if( dblTargetChambLen_mm[ii] < 0.0 ) continue;
WD40andTape 13:a373dfc57b89 188
WD40andTape 13:a373dfc57b89 189 _dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblADCCurrentPosition[ii]) / dblMaxRecalculatedTime;
WD40andTape 13:a373dfc57b89 190 dblVelocity_mmps[ii] = _dblVelocity_mmps[ii];
WD40andTape 6:f0a18e28a322 191 }
WD40andTape 6:f0a18e28a322 192 // SEND MESSAGE
dofydoink 11:7029367a1840 193 //dblMaxRecalculatedTime = 1.0;
WD40andTape 9:cd3607ba5643 194 hlcomms.make_message(&dblMaxRecalculatedTime);
WD40andTape 9:cd3607ba5643 195 error_code = hlcomms.send_message();
WD40andTape 6:f0a18e28a322 196 if( error_code < 0 ) {
dofydoink 12:595ed862e52f 197 if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 198 "Perhaps the server socket is not bound or not set to listen for connections? "
WD40andTape 6:f0a18e28a322 199 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 200 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 201 return;
WD40andTape 6:f0a18e28a322 202 }
WD40andTape 6:f0a18e28a322 203 }
WD40andTape 6:f0a18e28a322 204
dofydoink 0:607bc887b6e0 205 }
dofydoink 0:607bc887b6e0 206
WD40andTape 4:303584310071 207 void CalculateSmoothPath() {
dofydoink 5:712e7634c779 208 int jj;
WD40andTape 1:2a43cf183a62 209 double dblMeasuredSampleTime;
WD40andTape 7:5b6a2cefbf3b 210 double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; // The current position of the smooth path (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 211 //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 212 //double dblPressure_bar[N_CHANNELS]; // The pressure in a given chamber in bar (1 bar = 100,000 Pa)
WD40andTape 4:303584310071 213 while(1) {
dofydoink 0:607bc887b6e0 214 semPathPlan.wait();
WD40andTape 13:a373dfc57b89 215 //pinTesty = 1;
WD40andTape 1:2a43cf183a62 216 // If run time is more than 50 us from expected, calculate from measured time step
WD40andTape 4:303584310071 217 if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50) {
dofydoink 5:712e7634c779 218 dblMeasuredSampleTime = timer.read();
WD40andTape 4:303584310071 219 } else {
WD40andTape 1:2a43cf183a62 220 dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
WD40andTape 1:2a43cf183a62 221 }
WD40andTape 4:303584310071 222 timer.reset();
WD40andTape 1:2a43cf183a62 223
dofydoink 5:712e7634c779 224 for(jj = 0; jj < N_CHANNELS; jj++) {
WD40andTape 7:5b6a2cefbf3b 225 //dblPressure_bar[jj] = ReadADCPressure_bar(jj); // Read pressure from channel
WD40andTape 7:5b6a2cefbf3b 226 //dblPosition_mtrs[jj] = ReadADCPosition_mtrs(jj); // Read position from channel
WD40andTape 1:2a43cf183a62 227
WD40andTape 7:5b6a2cefbf3b 228 // Calculate next step in linear path
WD40andTape 7:5b6a2cefbf3b 229 mutPathIn.lock(); // Lock relevant mutex
WD40andTape 7:5b6a2cefbf3b 230 // Check tolerance
dofydoink 12:595ed862e52f 231 if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= FLT_PATH_TOLERANCE) {
WD40andTape 7:5b6a2cefbf3b 232 dblVelocity_mmps[jj] = 0.0; // Stop linear path generation when linear path is within tolerance of target position
WD40andTape 7:5b6a2cefbf3b 233 }
dofydoink 11:7029367a1840 234 dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime;
WD40andTape 15:59471daef4cb 235 dblLinearPathCurrentPos_mm[jj] = min( max( 0.0 , dblLinearPathCurrentPos_mm[jj] ) , 40.0 );
WD40andTape 7:5b6a2cefbf3b 236 mutPathIn.unlock(); // Unlock relevant mutex
dofydoink 0:607bc887b6e0 237
WD40andTape 7:5b6a2cefbf3b 238 // Calculate next step in smooth path
dofydoink 12:595ed862e52f 239 dblSmoothPathCurrentPos_mm[jj] = FLT_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0f-FLT_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
dofydoink 11:7029367a1840 240 dblSmoothPathCurrentPos_mm[jj] = max( 0.0 , dblSmoothPathCurrentPos_mm[jj] );
dofydoink 11:7029367a1840 241 llcomms.mutChannel[jj].lock(); // MUTEX LOCK
WD40andTape 10:1b6daba32452 242 llcomms.demandPosition[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);// Convert to a 13-bit number
WD40andTape 10:1b6daba32452 243 llcomms.demandPosition[jj] = llcomms.demandPosition[jj] & 0x1FFF; // Ensure number is 13-bit
WD40andTape 8:d6657767a182 244 llcomms.mutChannel[jj].unlock(); // MUTEX UNLOCK
dofydoink 0:607bc887b6e0 245
WD40andTape 10:1b6daba32452 246 llcomms.isDataReady[jj] = 1; // Signal that data ready
WD40andTape 4:303584310071 247 } // end for
WD40andTape 7:5b6a2cefbf3b 248
dofydoink 12:595ed862e52f 249 //if(IS_PRINT_OUTPUT) printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
dofydoink 12:595ed862e52f 250 //if(IS_PRINT_OUTPUT) printf("%f, %f, %f, %f, %f, %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblLinearPathCurrentPos_mm[1],dblLinearPathCurrentPos_mm[2],
dofydoink 11:7029367a1840 251 // dblLinearPathCurrentPos_mm[3],dblLinearPathCurrentPos_mm[4],dblLinearPathCurrentPos_mm[5],dblLinearPathCurrentPos_mm[6],dblLinearPathCurrentPos_mm[7]);
dofydoink 12:595ed862e52f 252 //if(IS_PRINT_OUTPUT) printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
WD40andTape 13:a373dfc57b89 253 //pinTesty = 0;
WD40andTape 4:303584310071 254 } // end while
WD40andTape 3:c83291bf9fd2 255 }
dofydoink 0:607bc887b6e0 256
WD40andTape 16:1e2804a4e5bd 257 //short int whichVar = 0;
WD40andTape 16:1e2804a4e5bd 258 void sendPressures() {
WD40andTape 16:1e2804a4e5bd 259 pc.printf("%f,%f\r\n",dblLinearPathCurrentPos_mm[1],llcomms.ReadADCPressure_bar(1));
WD40andTape 16:1e2804a4e5bd 260 /*switch(whichVar) {
WD40andTape 16:1e2804a4e5bd 261 case 0 : pc.printf("%f,",llcomms.ReadADCPressure_bar(???));
WD40andTape 16:1e2804a4e5bd 262 break;
WD40andTape 16:1e2804a4e5bd 263 case 1 : pc.printf("%f,",dblLinearPathCurrentPos_mm[???]);
WD40andTape 16:1e2804a4e5bd 264 break;
WD40andTape 16:1e2804a4e5bd 265 case 2 : pc.printf("%f,",llcomms.ReadADCPressure_bar(???));
WD40andTape 16:1e2804a4e5bd 266 break;
WD40andTape 16:1e2804a4e5bd 267 case 3 : pc.printf("%f,",dblLinearPathCurrentPos_mm[???]);
WD40andTape 16:1e2804a4e5bd 268 break;
WD40andTape 16:1e2804a4e5bd 269 case 4 : pc.printf("%f,",llcomms.ReadADCPressure_bar(???));
WD40andTape 16:1e2804a4e5bd 270 break;
WD40andTape 16:1e2804a4e5bd 271 case 5 : pc.printf("%f,",dblLinearPathCurrentPos_mm[???]);
WD40andTape 16:1e2804a4e5bd 272 break;
WD40andTape 16:1e2804a4e5bd 273 case 6 : pc.printf("%f,",llcomms.ReadADCPressure_bar(???));
WD40andTape 16:1e2804a4e5bd 274 break;
WD40andTape 16:1e2804a4e5bd 275 case 7 : pc.printf("%f,",dblLinearPathCurrentPos_mm[???]);
WD40andTape 16:1e2804a4e5bd 276 break;
WD40andTape 16:1e2804a4e5bd 277 case 0 : pc.printf("%f,",llcomms.ReadADCPressure_bar(???));
WD40andTape 16:1e2804a4e5bd 278 break;
WD40andTape 16:1e2804a4e5bd 279 case 1 : pc.printf("%f,",dblLinearPathCurrentPos_mm[???]);
WD40andTape 16:1e2804a4e5bd 280 break;
WD40andTape 16:1e2804a4e5bd 281 case 0 : pc.printf("%f,",llcomms.ReadADCPressure_bar(???));
WD40andTape 16:1e2804a4e5bd 282 break;
WD40andTape 16:1e2804a4e5bd 283 case 1 : pc.printf("%f,",dblLinearPathCurrentPos_mm[???]);
WD40andTape 16:1e2804a4e5bd 284 break;
WD40andTape 16:1e2804a4e5bd 285 }
WD40andTape 16:1e2804a4e5bd 286 if( whichVar < 8 ) {
WD40andTape 16:1e2804a4e5bd 287 whichVar++;
WD40andTape 16:1e2804a4e5bd 288 } else {
WD40andTape 16:1e2804a4e5bd 289 whichVar = 0;
WD40andTape 16:1e2804a4e5bd 290 }*/
WD40andTape 16:1e2804a4e5bd 291 }
WD40andTape 16:1e2804a4e5bd 292
WD40andTape 13:a373dfc57b89 293 int main() {
WD40andTape 7:5b6a2cefbf3b 294 pc.baud(BAUD_RATE);
WD40andTape 13:a373dfc57b89 295 printf("Sup bruvva! I'll be your mid-level controller for today.\r\n");
WD40andTape 13:a373dfc57b89 296 printf("Compiled at %s\r\n.",__TIME__);
dofydoink 5:712e7634c779 297 wait(3);
WD40andTape 9:cd3607ba5643 298
dofydoink 0:607bc887b6e0 299 timer.start();
WD40andTape 7:5b6a2cefbf3b 300
WD40andTape 10:1b6daba32452 301 threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
WD40andTape 7:5b6a2cefbf3b 302 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
WD40andTape 7:5b6a2cefbf3b 303 threadSmoothPathPlan.start(CalculateSmoothPath); // Start planning thread
WD40andTape 7:5b6a2cefbf3b 304
WD40andTape 7:5b6a2cefbf3b 305 PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); // Set up planning thread to recur at fixed intervals
WD40andTape 16:1e2804a4e5bd 306 SendPressuresTicker.attach(&sendPressures, 0.1); // Set up planning thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 307
WD40andTape 7:5b6a2cefbf3b 308 Thread::wait(1);
WD40andTape 1:2a43cf183a62 309 while(1) {
WD40andTape 1:2a43cf183a62 310 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 311 }
WD40andTape 7:5b6a2cefbf3b 312 }