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

Committer:
WD40andTape
Date:
Tue Aug 07 09:18:15 2018 +0000
Revision:
8:d6657767a182
Parent:
7:5b6a2cefbf3b
Child:
9:cd3607ba5643
Moved some of the LL communication code out to the "LLComms" class.

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 1:2a43cf183a62 58 bool isDataReady[N_CHANNELS]; // flag to indicate path data is ready for transmission to low level.
dofydoink 0:607bc887b6e0 59
WD40andTape 8:d6657767a182 60 /*double dblGlobalTest;
WD40andTape 8:d6657767a182 61 int intGlobalTest;*/
dofydoink 0:607bc887b6e0 62
WD40andTape 7:5b6a2cefbf3b 63 void startPathPlan() { // Plan a new linear path after receiving new target data
WD40andTape 4:303584310071 64 semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
dofydoink 0:607bc887b6e0 65 }
dofydoink 0:607bc887b6e0 66
WD40andTape 7:5b6a2cefbf3b 67 // This function will be called when a new transmission is received from high level
WD40andTape 7:5b6a2cefbf3b 68 void ReceiveAndReplan() {
WD40andTape 7:5b6a2cefbf3b 69 HLComms hlcomms(SERVER_PORT);
WD40andTape 6:f0a18e28a322 70
WD40andTape 6:f0a18e28a322 71 int error_code;
WD40andTape 7:5b6a2cefbf3b 72 error_code = hlcomms.setup_server();
WD40andTape 6:f0a18e28a322 73 if( error_code == -1 ) return;
WD40andTape 7:5b6a2cefbf3b 74 error_code = hlcomms.accept_connection();
WD40andTape 7:5b6a2cefbf3b 75 if( error_code == -1 ) {
WD40andTape 7:5b6a2cefbf3b 76 hlcomms.close_server();
WD40andTape 7:5b6a2cefbf3b 77 return;
WD40andTape 7:5b6a2cefbf3b 78 }
WD40andTape 6:f0a18e28a322 79
WD40andTape 7:5b6a2cefbf3b 80 int ii;
WD40andTape 6:f0a18e28a322 81 unsigned char recv_buffer[1024];
WD40andTape 7:5b6a2cefbf3b 82 struct msg_format input; //hlcomms.msg_format
WD40andTape 6:f0a18e28a322 83 char send_buffer[65];
WD40andTape 6:f0a18e28a322 84 char * message_to_send;
WD40andTape 4:303584310071 85
WD40andTape 6:f0a18e28a322 86 while( true ) {
WD40andTape 6:f0a18e28a322 87 // RECEIVE MESSAGE
WD40andTape 6:f0a18e28a322 88 memset(recv_buffer, 0, sizeof(recv_buffer));
WD40andTape 7:5b6a2cefbf3b 89 error_code = hlcomms.interfaces.clt_sock.recv(recv_buffer, 1024); // Blocks until data is received
WD40andTape 6:f0a18e28a322 90 if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
WD40andTape 6:f0a18e28a322 91 printf("Client disconnected.\n\r");
WD40andTape 7:5b6a2cefbf3b 92 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 93 return;
WD40andTape 6:f0a18e28a322 94 } else if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 95 printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 96 "Perhaps the server socket is not connected to a remote host? "
WD40andTape 6:f0a18e28a322 97 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 98 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 99 return;
WD40andTape 6:f0a18e28a322 100 }
WD40andTape 6:f0a18e28a322 101 //printf("Message received.\r\n");
WD40andTape 7:5b6a2cefbf3b 102 input = hlcomms.consume_message( recv_buffer );
WD40andTape 1:2a43cf183a62 103
WD40andTape 7:5b6a2cefbf3b 104 // PROCESS INPUT
WD40andTape 7:5b6a2cefbf3b 105 double dblTargetChambLen_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
WD40andTape 6:f0a18e28a322 106 printf("REPLAN, %f\r\n",input.duration);
WD40andTape 7:5b6a2cefbf3b 107 // Update front segment
WD40andTape 6:f0a18e28a322 108 dblTargetChambLen_mm[0] = input.psi[0][0]*1000;
WD40andTape 6:f0a18e28a322 109 dblTargetChambLen_mm[1] = input.psi[0][1]*1000;
WD40andTape 6:f0a18e28a322 110 dblTargetChambLen_mm[2] = input.psi[0][2]*1000;
WD40andTape 7:5b6a2cefbf3b 111 // Update mid segment
WD40andTape 6:f0a18e28a322 112 dblTargetChambLen_mm[6] = input.psi[1][0]*1000;
WD40andTape 7:5b6a2cefbf3b 113 dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; // Same because two pumps are used
WD40andTape 7:5b6a2cefbf3b 114 // Update rear segment
WD40andTape 6:f0a18e28a322 115 dblTargetChambLen_mm[3] = input.psi[2][0]*1000;
WD40andTape 6:f0a18e28a322 116 dblTargetChambLen_mm[4] = input.psi[2][1]*1000;
WD40andTape 6:f0a18e28a322 117 dblTargetChambLen_mm[5] = input.psi[2][2]*1000;
dofydoink 0:607bc887b6e0 118
WD40andTape 6:f0a18e28a322 119 bool isTimeChanged = 0;
WD40andTape 6:f0a18e28a322 120 double dblMaxRecalculatedTime = input.duration;
WD40andTape 7:5b6a2cefbf3b 121 mutPathIn.lock(); // Lock variables to avoid race condition
WD40andTape 4:303584310071 122 for (ii = 0; ii< N_CHANNELS; ii++) {
dofydoink 0:607bc887b6e0 123 //check to see if positions are achievable
WD40andTape 6:f0a18e28a322 124 /*if(dblTargetChambLen_mm[ii]>DBL_MAX_CHAMBER_LENGTHS_MM[ii]) {
WD40andTape 4:303584310071 125 dblTargetChambLen_mm[ii] = DBL_MAX_CHAMBER_LENGTHS_MM[ii];
WD40andTape 6:f0a18e28a322 126 isTimeChanged = 1;
dofydoink 0:607bc887b6e0 127 }
WD40andTape 4:303584310071 128 if(dblTargetChambLen_mm[ii]<0.0) {
WD40andTape 6:f0a18e28a322 129 dblTargetChambLen_mm[ii] = 0.0;
WD40andTape 6:f0a18e28a322 130 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 131 }*/
WD40andTape 6:f0a18e28a322 132 dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
WD40andTape 6:f0a18e28a322 133 //!! LIMIT CHAMBER LENGTHS TOO
WD40andTape 6:f0a18e28a322 134 if( dblTargetActPos_mm[ii]<0.0 || dblTargetActPos_mm[ii]>25.0 ) {
WD40andTape 6:f0a18e28a322 135 dblTargetActPos_mm[ii] = min( max( 0.0 , dblTargetActPos_mm[ii] ) , 25.0 );
WD40andTape 6:f0a18e28a322 136 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 137 }
WD40andTape 6:f0a18e28a322 138 }
WD40andTape 6:f0a18e28a322 139 double dblActPosChange;
WD40andTape 6:f0a18e28a322 140 short sgn;
WD40andTape 7:5b6a2cefbf3b 141 for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
WD40andTape 6:f0a18e28a322 142 dblActPosChange = dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii];
WD40andTape 6:f0a18e28a322 143 if( fabs(dblActPosChange) < DBL_PATH_TOLERANCE ) {
WD40andTape 6:f0a18e28a322 144 dblActPosChange = 0.0;
WD40andTape 6:f0a18e28a322 145 isTimeChanged = 1;
dofydoink 0:607bc887b6e0 146 }
WD40andTape 6:f0a18e28a322 147 if( input.duration < 0.000000001 ) {
WD40andTape 6:f0a18e28a322 148 sgn = (dblActPosChange > 0) ? 1 : ((dblActPosChange < 0) ? -1 : 0);
WD40andTape 6:f0a18e28a322 149 dblVelocity_mmps[ii] = sgn * MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 150 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 151 } else {
WD40andTape 6:f0a18e28a322 152 dblVelocity_mmps[ii] = dblActPosChange / input.duration;
WD40andTape 6:f0a18e28a322 153 }
WD40andTape 7:5b6a2cefbf3b 154 // Check to see if velocities are achievable
WD40andTape 6:f0a18e28a322 155 if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
WD40andTape 6:f0a18e28a322 156 if(dblVelocity_mmps[ii]>0) {
WD40andTape 6:f0a18e28a322 157 dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 158 } else {
WD40andTape 6:f0a18e28a322 159 dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 160 }
WD40andTape 6:f0a18e28a322 161 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 162 }
WD40andTape 6:f0a18e28a322 163 double dblRecalculatedTime;
WD40andTape 6:f0a18e28a322 164 if( fabs(dblVelocity_mmps[ii]) < 0.000000001 ) {
WD40andTape 6:f0a18e28a322 165 dblRecalculatedTime = 0;
WD40andTape 6:f0a18e28a322 166 } else {
WD40andTape 6:f0a18e28a322 167 dblRecalculatedTime = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblVelocity_mmps[ii];
WD40andTape 6:f0a18e28a322 168 }
WD40andTape 6:f0a18e28a322 169 if( dblRecalculatedTime > dblMaxRecalculatedTime ) {
WD40andTape 6:f0a18e28a322 170 dblMaxRecalculatedTime = dblRecalculatedTime;
WD40andTape 6:f0a18e28a322 171 }
WD40andTape 6:f0a18e28a322 172 }
WD40andTape 6:f0a18e28a322 173 if( isTimeChanged ) {
WD40andTape 6:f0a18e28a322 174 if( dblMaxRecalculatedTime < input.duration ) {
WD40andTape 6:f0a18e28a322 175 dblMaxRecalculatedTime = input.duration;
WD40andTape 6:f0a18e28a322 176 }
WD40andTape 7:5b6a2cefbf3b 177 for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
WD40andTape 6:f0a18e28a322 178 dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblMaxRecalculatedTime;
WD40andTape 6:f0a18e28a322 179 }
WD40andTape 6:f0a18e28a322 180 }
WD40andTape 7:5b6a2cefbf3b 181 mutPathIn.unlock(); // Unlock mutex
WD40andTape 6:f0a18e28a322 182
WD40andTape 6:f0a18e28a322 183 printf("Sending message...\r\n");
WD40andTape 6:f0a18e28a322 184 // SEND MESSAGE
WD40andTape 6:f0a18e28a322 185 memset(send_buffer, 0, sizeof(send_buffer));
WD40andTape 6:f0a18e28a322 186 _snprintf(send_buffer,64,"%f",dblMaxRecalculatedTime);
WD40andTape 6:f0a18e28a322 187 message_to_send = send_buffer;
WD40andTape 7:5b6a2cefbf3b 188 error_code = hlcomms.interfaces.clt_sock.send(message_to_send, strlen(message_to_send));
WD40andTape 6:f0a18e28a322 189 if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 190 printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 191 "Perhaps the server socket is not bound or not set to listen for connections? "
WD40andTape 6:f0a18e28a322 192 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 193 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 194 return;
WD40andTape 6:f0a18e28a322 195 }
WD40andTape 6:f0a18e28a322 196 printf("Message sent.\r\n");
WD40andTape 6:f0a18e28a322 197 }
WD40andTape 6:f0a18e28a322 198
dofydoink 0:607bc887b6e0 199 }
dofydoink 0:607bc887b6e0 200
WD40andTape 4:303584310071 201 void CalculateSmoothPath() {
dofydoink 5:712e7634c779 202 int jj;
WD40andTape 1:2a43cf183a62 203 double dblMeasuredSampleTime;
WD40andTape 7:5b6a2cefbf3b 204 double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; // The current position of the smooth path (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 205 //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 206 //double dblPressure_bar[N_CHANNELS]; // The pressure in a given chamber in bar (1 bar = 100,000 Pa)
WD40andTape 4:303584310071 207 while(1) {
dofydoink 0:607bc887b6e0 208 semPathPlan.wait();
WD40andTape 1:2a43cf183a62 209
WD40andTape 1:2a43cf183a62 210 // If run time is more than 50 us from expected, calculate from measured time step
WD40andTape 4:303584310071 211 if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50) {
dofydoink 5:712e7634c779 212 dblMeasuredSampleTime = timer.read();
WD40andTape 4:303584310071 213 } else {
WD40andTape 1:2a43cf183a62 214 dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
WD40andTape 1:2a43cf183a62 215 }
WD40andTape 4:303584310071 216 timer.reset();
WD40andTape 1:2a43cf183a62 217
dofydoink 5:712e7634c779 218 for(jj = 0; jj < N_CHANNELS; jj++) {
WD40andTape 7:5b6a2cefbf3b 219 //dblPressure_bar[jj] = ReadADCPressure_bar(jj); // Read pressure from channel
WD40andTape 7:5b6a2cefbf3b 220 //dblPosition_mtrs[jj] = ReadADCPosition_mtrs(jj); // Read position from channel
WD40andTape 1:2a43cf183a62 221
WD40andTape 7:5b6a2cefbf3b 222 // Calculate next step in linear path
WD40andTape 7:5b6a2cefbf3b 223 mutPathIn.lock(); // Lock relevant mutex
WD40andTape 7:5b6a2cefbf3b 224 dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime;
WD40andTape 7:5b6a2cefbf3b 225 if(dblLinearPathCurrentPos_mm[jj] < 0.0) {
WD40andTape 7:5b6a2cefbf3b 226 dblLinearPathCurrentPos_mm[jj] = 0.00;
WD40andTape 7:5b6a2cefbf3b 227 }
WD40andTape 7:5b6a2cefbf3b 228 // Check tolerance
WD40andTape 7:5b6a2cefbf3b 229 if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= DBL_PATH_TOLERANCE) {
WD40andTape 7:5b6a2cefbf3b 230 dblVelocity_mmps[jj] = 0.0; // Stop linear path generation when linear path is within tolerance of target position
WD40andTape 7:5b6a2cefbf3b 231 }
WD40andTape 7:5b6a2cefbf3b 232 mutPathIn.unlock(); // Unlock relevant mutex
dofydoink 0:607bc887b6e0 233
WD40andTape 7:5b6a2cefbf3b 234 // Calculate next step in smooth path
dofydoink 5:712e7634c779 235 dblSmoothPathCurrentPos_mm[jj] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
WD40andTape 8:d6657767a182 236 llcomms.mutChannel[jj].lock(); // MUTEX LOCK
WD40andTape 7:5b6a2cefbf3b 237 intDemandPos_Tx[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);// Convert to a 13-bit number
WD40andTape 7:5b6a2cefbf3b 238 intDemandPos_Tx[jj] = intDemandPos_Tx[jj] & 0x1FFF; // Ensure number is 13-bit
WD40andTape 8:d6657767a182 239 llcomms.mutChannel[jj].unlock(); // MUTEX UNLOCK
dofydoink 0:607bc887b6e0 240
dofydoink 5:712e7634c779 241 isDataReady[jj] = 1;//signal that data ready
WD40andTape 4:303584310071 242 } // end for
WD40andTape 7:5b6a2cefbf3b 243
WD40andTape 6:f0a18e28a322 244 //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
WD40andTape 6:f0a18e28a322 245 /*printf("%f, %f, %f, %f, %f, %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblLinearPathCurrentPos_mm[1],dblLinearPathCurrentPos_mm[2],
WD40andTape 6:f0a18e28a322 246 dblLinearPathCurrentPos_mm[3],dblLinearPathCurrentPos_mm[4],dblLinearPathCurrentPos_mm[5],dblLinearPathCurrentPos_mm[6],dblLinearPathCurrentPos_mm[7]);*/
WD40andTape 6:f0a18e28a322 247 //printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
WD40andTape 7:5b6a2cefbf3b 248
WD40andTape 4:303584310071 249 } // end while
WD40andTape 3:c83291bf9fd2 250 }
dofydoink 0:607bc887b6e0 251
WD40andTape 8:d6657767a182 252 // NEED TO FIGURE OUT POINTER-TO-MEMBER FUNCTIONS TO MOVE THESE INTO LLComms CLASS
WD40andTape 8:d6657767a182 253 void SendReceiveData(int channel, int _intDemandPos_Tx[], bool (*_isDataReady)[8]) {
WD40andTape 8:d6657767a182 254 int intPosSPI_Rx[N_CHANNELS]; // 13 bit value received over SPI from the actuator
WD40andTape 8:d6657767a182 255
WD40andTape 8:d6657767a182 256 // Get data from controller
WD40andTape 8:d6657767a182 257 llcomms.spi.format(16,2);
WD40andTape 8:d6657767a182 258 llcomms.spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 8:d6657767a182 259 llcomms.mutChannel[channel].lock(); // Lock mutex for specific Channel
WD40andTape 8:d6657767a182 260 *llcomms.cs_LL[channel] = 0; // Select relevant chip
WD40andTape 8:d6657767a182 261 intPosSPI_Rx[channel] = llcomms.spi.write(_intDemandPos_Tx[channel]); // Transmit & receive
WD40andTape 8:d6657767a182 262 *llcomms.cs_LL[channel] = 1; // Deselect chip
WD40andTape 8:d6657767a182 263 *_isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
WD40andTape 8:d6657767a182 264 /*if(channel == 0) {
WD40andTape 8:d6657767a182 265 intGlobalTest = intPosSPI_Rx[channel];
WD40andTape 8:d6657767a182 266 dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
WD40andTape 8:d6657767a182 267 }*/
WD40andTape 8:d6657767a182 268
WD40andTape 8:d6657767a182 269 // Sort out received data
WD40andTape 8:d6657767a182 270 llcomms.chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
WD40andTape 8:d6657767a182 271
WD40andTape 8:d6657767a182 272 intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
WD40andTape 8:d6657767a182 273 //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
WD40andTape 8:d6657767a182 274 llcomms.mutChannel[channel].unlock();//unlock mutex for specific channel
WD40andTape 8:d6657767a182 275 //printf("%d, %d\r\n",intPosSPI_Rx[0],_intDemandPos_Tx[0]);
WD40andTape 8:d6657767a182 276 }
WD40andTape 8:d6657767a182 277 // Common rise handler function
WD40andTape 8:d6657767a182 278 void common_rise_handler(int channel) {
WD40andTape 8:d6657767a182 279 llcomms.pinCheck = 1;
WD40andTape 8:d6657767a182 280 if (isDataReady[channel]) { // Check if data is ready for tranmission
WD40andTape 8:d6657767a182 281 //llcomms.ThreadID[channel] = queue.call(&llcomms.SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 8:d6657767a182 282 //llcomms.ThreadID[channel] = queue.call(&llcomms.SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 8:d6657767a182 283 //llcomms.ThreadID[channel] = queue.call(mbed::Callback(&llcomms,&LLComms::SendReceiveData),channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 8:d6657767a182 284 // llcomms.ThreadID[channel] = queue.call(mbed::Callback<void()>(&llcomms,&LLComms::SendReceiveData),channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 8:d6657767a182 285 //llcomms.ThreadID[channel] = queue.call(&llcomms,*llcomms.SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 8:d6657767a182 286 llcomms.ThreadID[channel] = queue.call(&SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
WD40andTape 8:d6657767a182 287 //callback(&low_pass1, &LowPass::step)
WD40andTape 8:d6657767a182 288 }
WD40andTape 8:d6657767a182 289 }
WD40andTape 8:d6657767a182 290 // Common fall handler functions
WD40andTape 8:d6657767a182 291 void common_fall_handler(int channel) {
WD40andTape 8:d6657767a182 292 llcomms.pinCheck = 0;
WD40andTape 8:d6657767a182 293 queue.cancel(llcomms.ThreadID[channel]); // Cancel relevant queued event
WD40andTape 8:d6657767a182 294 }
WD40andTape 8:d6657767a182 295 // Stub rise functions
WD40andTape 8:d6657767a182 296 void rise0(void) { common_rise_handler(0); }
WD40andTape 8:d6657767a182 297 void rise1(void) { common_rise_handler(1); }
WD40andTape 8:d6657767a182 298 void rise2(void) { common_rise_handler(2); }
WD40andTape 8:d6657767a182 299 void rise3(void) { common_rise_handler(3); }
WD40andTape 8:d6657767a182 300 void rise4(void) { common_rise_handler(4); }
WD40andTape 8:d6657767a182 301 void rise5(void) { common_rise_handler(5); }
WD40andTape 8:d6657767a182 302 void rise6(void) { common_rise_handler(6); }
WD40andTape 8:d6657767a182 303 void rise7(void) { common_rise_handler(7); }
WD40andTape 8:d6657767a182 304 // Stub fall functions
WD40andTape 8:d6657767a182 305 void fall0(void) { common_fall_handler(0); }
WD40andTape 8:d6657767a182 306 void fall1(void) { common_fall_handler(1); }
WD40andTape 8:d6657767a182 307 void fall2(void) { common_fall_handler(2); }
WD40andTape 8:d6657767a182 308 void fall3(void) { common_fall_handler(3); }
WD40andTape 8:d6657767a182 309 void fall4(void) { common_fall_handler(4); }
WD40andTape 8:d6657767a182 310 void fall5(void) { common_fall_handler(5); }
WD40andTape 8:d6657767a182 311 void fall6(void) { common_fall_handler(6); }
WD40andTape 8:d6657767a182 312 void fall7(void) { common_fall_handler(7); }
WD40andTape 8:d6657767a182 313
WD40andTape 7:5b6a2cefbf3b 314 int main() {
WD40andTape 4:303584310071 315 int ii;
WD40andTape 7:5b6a2cefbf3b 316 // Initialise relevant variables
WD40andTape 7:5b6a2cefbf3b 317 for(ii = 0; ii<N_CHANNELS; ii++) {
WD40andTape 7:5b6a2cefbf3b 318 // All chip selects in off state
WD40andTape 8:d6657767a182 319 *llcomms.cs_LL[ii] = 1;
WD40andTape 8:d6657767a182 320 *llcomms.cs_ADC[ii] = 1;
WD40andTape 7:5b6a2cefbf3b 321 // Data ready flags set to not ready
WD40andTape 1:2a43cf183a62 322 isDataReady[ii] = 0;
WD40andTape 7:5b6a2cefbf3b 323 }
dofydoink 0:607bc887b6e0 324
WD40andTape 8:d6657767a182 325 llcomms.pinReset = 1; // Initialise reset pin to not reset the controllers.
dofydoink 5:712e7634c779 326 wait(0.25);
WD40andTape 8:d6657767a182 327 llcomms.pinReset=0; // Reset controllers to be safe
dofydoink 5:712e7634c779 328 wait(0.25);
WD40andTape 8:d6657767a182 329 llcomms.pinReset = 1; // Ready to go
dofydoink 0:607bc887b6e0 330
WD40andTape 7:5b6a2cefbf3b 331 pc.baud(BAUD_RATE);
dofydoink 0:607bc887b6e0 332 printf("Hi, there! I'll be your mid-level controller for today.\r\n");
dofydoink 5:712e7634c779 333 wait(3);
WD40andTape 7:5b6a2cefbf3b 334 t.start(callback(&queue, &EventQueue::dispatch_forever)); // Start the event queue
dofydoink 0:607bc887b6e0 335
WD40andTape 7:5b6a2cefbf3b 336 // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 8:d6657767a182 337 llcomms.pinGate0.rise(&rise0);
WD40andTape 8:d6657767a182 338 llcomms.pinGate1.rise(&rise1);
WD40andTape 8:d6657767a182 339 llcomms.pinGate2.rise(&rise2);
WD40andTape 8:d6657767a182 340 llcomms.pinGate3.rise(&rise3);
WD40andTape 8:d6657767a182 341 llcomms.pinGate4.rise(&rise4);
WD40andTape 8:d6657767a182 342 llcomms.pinGate5.rise(&rise5);
WD40andTape 8:d6657767a182 343 llcomms.pinGate6.rise(&rise6);
WD40andTape 8:d6657767a182 344 llcomms.pinGate7.rise(&rise7);
WD40andTape 7:5b6a2cefbf3b 345 // Set up fall interrupts MIGHT NOT NEED TO BE POINTERS
WD40andTape 8:d6657767a182 346 llcomms.pinGate0.fall(&fall0);
WD40andTape 8:d6657767a182 347 llcomms.pinGate1.fall(&fall1);
WD40andTape 8:d6657767a182 348 llcomms.pinGate2.fall(&fall2);
WD40andTape 8:d6657767a182 349 llcomms.pinGate3.fall(&fall3);
WD40andTape 8:d6657767a182 350 llcomms.pinGate4.fall(&fall4);
WD40andTape 8:d6657767a182 351 llcomms.pinGate5.fall(&fall5);
WD40andTape 8:d6657767a182 352 llcomms.pinGate6.fall(&fall6);
WD40andTape 8:d6657767a182 353 llcomms.pinGate7.fall(&fall7);
dofydoink 5:712e7634c779 354
dofydoink 0:607bc887b6e0 355 timer.start();
WD40andTape 7:5b6a2cefbf3b 356
WD40andTape 7:5b6a2cefbf3b 357 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
WD40andTape 7:5b6a2cefbf3b 358 threadSmoothPathPlan.start(CalculateSmoothPath); // Start planning thread
WD40andTape 7:5b6a2cefbf3b 359
WD40andTape 7:5b6a2cefbf3b 360 PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); // Set up planning thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 361
WD40andTape 7:5b6a2cefbf3b 362 Thread::wait(1);
WD40andTape 1:2a43cf183a62 363 while(1) {
WD40andTape 1:2a43cf183a62 364 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 365 }
WD40andTape 7:5b6a2cefbf3b 366 }