MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

main.cpp@21:0b10d8e615d1, 2018-12-11 (annotated)

Committer:
WD40andTape
Date:
Tue Dec 11 15:45:01 2018 +0000
Revision:
21:0b10d8e615d1
Parent:
20:b2139294c547
Child:
22:82871f00f89d
Updates to main to move path following to LL

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
dofydoink 12:595ed862e52f 11 // Maximum achievable mm path tolerance plus additional % tolerance
WD40andTape 21:0b10d8e615d1 12 const float FLT_PATH_TOLERANCE = MAX_SPEED_MMPS * (1/LL_DEMANDS_FREQ_HZ) * (1.0f+FLT_PERCENT_PATH_TOLERANCE);
dofydoink 0:607bc887b6e0 13
WD40andTape 21:0b10d8e615d1 14 // DEMAND VARIABLES
WD40andTape 21:0b10d8e615d1 15 double dblDemandVelocity_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
WD40andTape 21:0b10d8e615d1 16 double dblDemandPosition_mtrs[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
WD40andTape 21:0b10d8e615d1 17 // SENSOR VARIABLES
WD40andTape 21:0b10d8e615d1 18 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 21:0b10d8e615d1 19 double dblPressure_bar[N_CHANNELS]; // The pressure in a given chamber in bar (1 bar = 100,000 Pa)
dofydoink 11:7029367a1840 20
WD40andTape 7:5b6a2cefbf3b 21 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
dofydoink 12:595ed862e52f 22 LLComms llcomms;
WD40andTape 16:1e2804a4e5bd 23 HLComms hlcomms(SERVER_PORT);
WD40andTape 3:c83291bf9fd2 24
WD40andTape 10:1b6daba32452 25 Thread threadLowLevelSPI(osPriorityRealtime);
WD40andTape 21:0b10d8e615d1 26 Thread threadSetDemands(osPriorityNormal);
WD40andTape 4:303584310071 27 Thread threadReceiveAndReplan(osPriorityBelowNormal);
WD40andTape 17:bbaf3e8440ad 28 Thread threadSensorFeedback(osPriorityBelowNormal);
dofydoink 0:607bc887b6e0 29
WD40andTape 4:303584310071 30 Mutex mutPathIn;
WD40andTape 21:0b10d8e615d1 31 Semaphore semLLcomms(1);
WD40andTape 17:bbaf3e8440ad 32 Semaphore semSensorData(1);
dofydoink 0:607bc887b6e0 33
WD40andTape 21:0b10d8e615d1 34 Ticker setDemandsTicker;
WD40andTape 17:bbaf3e8440ad 35 Ticker SendSensorDataTicker;
WD40andTape 17:bbaf3e8440ad 36
WD40andTape 17:bbaf3e8440ad 37
WD40andTape 17:bbaf3e8440ad 38 void sendSensorData() {
WD40andTape 17:bbaf3e8440ad 39 while( true ) {
WD40andTape 17:bbaf3e8440ad 40 semSensorData.wait();
WD40andTape 21:0b10d8e615d1 41 int error_code = hlcomms.send_sensor_message(dblPosition_mtrs,dblPressure_bar);
WD40andTape 17:bbaf3e8440ad 42 /*if( error_code < 0 ) {
WD40andTape 17:bbaf3e8440ad 43 if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 17:bbaf3e8440ad 44 "Perhaps the server socket is not bound or not set to listen for connections? "
WD40andTape 17:bbaf3e8440ad 45 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 17:bbaf3e8440ad 46 hlcomms.close_server();
WD40andTape 17:bbaf3e8440ad 47 return;
WD40andTape 17:bbaf3e8440ad 48 }*/
WD40andTape 17:bbaf3e8440ad 49 }
WD40andTape 17:bbaf3e8440ad 50 }
WD40andTape 17:bbaf3e8440ad 51
WD40andTape 17:bbaf3e8440ad 52 void signalSendSensorData() {
WD40andTape 17:bbaf3e8440ad 53 semSensorData.release();
WD40andTape 17:bbaf3e8440ad 54 }
dofydoink 0:607bc887b6e0 55
WD40andTape 21:0b10d8e615d1 56 void startLLcomms() { // Plan a new linear path after receiving new target data
WD40andTape 21:0b10d8e615d1 57 semLLcomms.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
dofydoink 0:607bc887b6e0 58 }
dofydoink 0:607bc887b6e0 59
WD40andTape 7:5b6a2cefbf3b 60 // This function will be called when a new transmission is received from high level
WD40andTape 7:5b6a2cefbf3b 61 void ReceiveAndReplan() {
WD40andTape 6:f0a18e28a322 62
WD40andTape 6:f0a18e28a322 63 int error_code;
WD40andTape 7:5b6a2cefbf3b 64 error_code = hlcomms.setup_server();
WD40andTape 6:f0a18e28a322 65 if( error_code == -1 ) return;
WD40andTape 7:5b6a2cefbf3b 66 error_code = hlcomms.accept_connection();
WD40andTape 7:5b6a2cefbf3b 67 if( error_code == -1 ) {
WD40andTape 7:5b6a2cefbf3b 68 hlcomms.close_server();
WD40andTape 7:5b6a2cefbf3b 69 return;
WD40andTape 7:5b6a2cefbf3b 70 }
WD40andTape 6:f0a18e28a322 71
WD40andTape 18:6533fb7f5a91 72 SendSensorDataTicker.attach(&signalSendSensorData, 0.1); // Set up planning thread to recur at fixed intervals
WD40andTape 17:bbaf3e8440ad 73
WD40andTape 7:5b6a2cefbf3b 74 struct msg_format input; //hlcomms.msg_format
WD40andTape 4:303584310071 75
WD40andTape 6:f0a18e28a322 76 while( true ) {
WD40andTape 6:f0a18e28a322 77 // RECEIVE MESSAGE
WD40andTape 9:cd3607ba5643 78 error_code = hlcomms.receive_message();
WD40andTape 6:f0a18e28a322 79 if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
dofydoink 12:595ed862e52f 80 if(IS_PRINT_OUTPUT) printf("Client disconnected.\n\r");
WD40andTape 7:5b6a2cefbf3b 81 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 82 return;
WD40andTape 6:f0a18e28a322 83 } else if( error_code < 0 ) {
dofydoink 12:595ed862e52f 84 if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 85 "Perhaps the server socket is not connected to a remote host? "
WD40andTape 6:f0a18e28a322 86 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 87 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 88 return;
WD40andTape 6:f0a18e28a322 89 }
WD40andTape 17:bbaf3e8440ad 90 // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! RECV MUTEX
WD40andTape 9:cd3607ba5643 91 input = hlcomms.process_message();
WD40andTape 1:2a43cf183a62 92
WD40andTape 7:5b6a2cefbf3b 93 // PROCESS INPUT
WD40andTape 19:e5acb2183d4e 94 if(IS_PRINT_OUTPUT) printf("REPLAN, %f\r\n",input.speed);
WD40andTape 14:54c3759e76ed 95
WD40andTape 19:e5acb2183d4e 96 double dblTarget_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
WD40andTape 19:e5acb2183d4e 97 // Update rear segment
WD40andTape 19:e5acb2183d4e 98 dblTarget_mm[3] = input.psi[0][0]*1000;
WD40andTape 19:e5acb2183d4e 99 dblTarget_mm[5] = 0.0; //input.psi[0][1]*1000;
WD40andTape 19:e5acb2183d4e 100 dblTarget_mm[6] = 0.0; //input.psi[0][2]*1000;
WD40andTape 19:e5acb2183d4e 101 // Update mid segment
WD40andTape 19:e5acb2183d4e 102 dblTarget_mm[4] = input.psi[1][0]*1000;
WD40andTape 19:e5acb2183d4e 103 dblTarget_mm[7] = dblTarget_mm[4]; // Same because two pumps are used
WD40andTape 14:54c3759e76ed 104 // Update front segment
WD40andTape 19:e5acb2183d4e 105 dblTarget_mm[0] = input.psi[2][0]*1000;
WD40andTape 19:e5acb2183d4e 106 dblTarget_mm[1] = input.psi[2][1]*1000;
WD40andTape 19:e5acb2183d4e 107 dblTarget_mm[2] = input.psi[2][2]*1000;
dofydoink 11:7029367a1840 108
WD40andTape 21:0b10d8e615d1 109 // Lock mutex, preventing setDemandsForLL from running
WD40andTape 19:e5acb2183d4e 110 mutPathIn.lock();
WD40andTape 19:e5acb2183d4e 111 // Limit requested speed
WD40andTape 19:e5acb2183d4e 112 double limitedSpeed_mmps = min( max( 0.0 , input.speed ) , (double)MAX_SPEED_MMPS );
WD40andTape 19:e5acb2183d4e 113 // For each actuator, limit the input position, calculate the position change, and select the absolute max
WD40andTape 19:e5acb2183d4e 114 double dblDisplacementToTarget_mm[N_CHANNELS];
WD40andTape 19:e5acb2183d4e 115 double maxDistanceToTarget_mm = 0.0;
WD40andTape 19:e5acb2183d4e 116 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 19:e5acb2183d4e 117 // If requested position is negative
WD40andTape 19:e5acb2183d4e 118 if(dblTarget_mm[i]<0) {
WD40andTape 19:e5acb2183d4e 119 // Set actuator position change to 0
WD40andTape 19:e5acb2183d4e 120 dblDisplacementToTarget_mm[i] = 0.0;
WD40andTape 19:e5acb2183d4e 121 } else { // Requested position is positive
WD40andTape 19:e5acb2183d4e 122 // ? Limit requested chamber lengths
WD40andTape 19:e5acb2183d4e 123 // ? Convert from chamber length to actuator space
WD40andTape 19:e5acb2183d4e 124 // Limit actuator position
WD40andTape 21:0b10d8e615d1 125 dblTarget_mm[i] = min( max( 0.0 , dblTarget_mm[i] ) , (double)MAX_ACTUATOR_LIMIT ); // !!! USE A CONSTANT FOR THIS
WD40andTape 19:e5acb2183d4e 126 // Calculate actuator position change
WD40andTape 21:0b10d8e615d1 127 double dblCurrentPosition = dblPosition_mtrs[i];
WD40andTape 19:e5acb2183d4e 128 dblDisplacementToTarget_mm[i] = dblTarget_mm[i] - dblCurrentPosition;
WD40andTape 19:e5acb2183d4e 129 // Select the max absolute actuator position change
WD40andTape 19:e5acb2183d4e 130 if(fabs(dblDisplacementToTarget_mm[i])>maxDistanceToTarget_mm) {
WD40andTape 19:e5acb2183d4e 131 maxDistanceToTarget_mm = fabs(dblDisplacementToTarget_mm[i]);
WD40andTape 19:e5acb2183d4e 132 }
WD40andTape 19:e5acb2183d4e 133 }
dofydoink 11:7029367a1840 134 }
WD40andTape 19:e5acb2183d4e 135 // For max actuator position change, calculate the time to destination at the limited speed
WD40andTape 19:e5acb2183d4e 136 double maxTimeToTarget_s = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
WD40andTape 19:e5acb2183d4e 137 // For each actuator, replan target position and velocity as required
WD40andTape 19:e5acb2183d4e 138 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 19:e5acb2183d4e 139 // If requested actuator position change is already within tolerance, do NOT replan that actuator
WD40andTape 19:e5acb2183d4e 140 if( fabs(dblDisplacementToTarget_mm[i]) < FLT_PATH_TOLERANCE ) continue;
WD40andTape 19:e5acb2183d4e 141 // Calculate velocity for each motor to synchronise movements to complete in max time
WD40andTape 21:0b10d8e615d1 142 // Set dblDemandPosition_mtrs and dblDemandVelocity_mmps
WD40andTape 21:0b10d8e615d1 143 dblDemandPosition_mtrs[i] = dblTarget_mm[i];
WD40andTape 21:0b10d8e615d1 144 dblDemandVelocity_mmps[i] = dblDisplacementToTarget_mm[i] / maxTimeToTarget_s;
WD40andTape 19:e5acb2183d4e 145 }
WD40andTape 21:0b10d8e615d1 146 // Unlock mutex, allowing setDemandsForLL to run again
WD40andTape 19:e5acb2183d4e 147 mutPathIn.unlock();
dofydoink 12:595ed862e52f 148
WD40andTape 6:f0a18e28a322 149 // SEND MESSAGE
WD40andTape 19:e5acb2183d4e 150 error_code = hlcomms.send_duration_message(&maxTimeToTarget_s);
WD40andTape 6:f0a18e28a322 151 if( error_code < 0 ) {
dofydoink 12:595ed862e52f 152 if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 153 "Perhaps the server socket is not bound or not set to listen for connections? "
WD40andTape 6:f0a18e28a322 154 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 155 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 156 return;
WD40andTape 6:f0a18e28a322 157 }
WD40andTape 6:f0a18e28a322 158 }
WD40andTape 6:f0a18e28a322 159
dofydoink 0:607bc887b6e0 160 }
dofydoink 0:607bc887b6e0 161
WD40andTape 21:0b10d8e615d1 162 void setDemandsForLL() {
WD40andTape 21:0b10d8e615d1 163
WD40andTape 4:303584310071 164 while(1) {
WD40andTape 21:0b10d8e615d1 165 semLLcomms.wait();
WD40andTape 21:0b10d8e615d1 166
WD40andTape 21:0b10d8e615d1 167 mutPathIn.lock(); // Lock relevant mutex
WD40andTape 21:0b10d8e615d1 168 for(short int i=0; i<N_CHANNELS; i++) { // For each LL
WD40andTape 21:0b10d8e615d1 169 llcomms.mutChannel[i].lock(); // MUTEX LOCK
WD40andTape 21:0b10d8e615d1 170 // DO IT IN ONE BLOCK
WD40andTape 21:0b10d8e615d1 171 // SEND AND RECEIVE DATA AT SAME TIME
WD40andTape 21:0b10d8e615d1 172 // PASS RECEIVED DATA BACK
WD40andTape 21:0b10d8e615d1 173 // Send position to LL, receive position and store in global
WD40andTape 21:0b10d8e615d1 174 llcomms.demandPosition[i] = (int) ((dblDemandPosition_mtrs[i]/MAX_ACTUATOR_LENGTH)*8191); // Convert to a 13-bit number
WD40andTape 21:0b10d8e615d1 175 llcomms.demandPosition[i] = llcomms.demandPosition[i] & 0x1FFF; // Ensure number is 13-bit
WD40andTape 21:0b10d8e615d1 176 // Send speed to LL, receive pressure and store in global
WD40andTape 21:0b10d8e615d1 177 llcomms.demandSpeed[i] = (int) ((dblDemandVelocity_mmps[i]/MAX_ACTUATOR_SPEED)*8191);// Convert to a 13-bit number
WD40andTape 21:0b10d8e615d1 178 llcomms.demandSpeed[i] = llcomms.demandSpeed[i] & 0x1FFF; // Ensure number is 13-bit
WD40andTape 21:0b10d8e615d1 179 llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
WD40andTape 21:0b10d8e615d1 180 } // end for
WD40andTape 21:0b10d8e615d1 181 mutPathIn.unlock(); // Unlock relevant mutex
dofydoink 0:607bc887b6e0 182
WD40andTape 21:0b10d8e615d1 183 llcomms.isDataReady[i] = 1; // Signal that data ready
WD40andTape 21:0b10d8e615d1 184 } // end while
WD40andTape 7:5b6a2cefbf3b 185
WD40andTape 3:c83291bf9fd2 186 }
dofydoink 0:607bc887b6e0 187
WD40andTape 13:a373dfc57b89 188 int main() {
WD40andTape 7:5b6a2cefbf3b 189 pc.baud(BAUD_RATE);
WD40andTape 17:bbaf3e8440ad 190 printf("ML engage. Compiled at %s\r\n.",__TIME__);
dofydoink 5:712e7634c779 191 wait(3);
WD40andTape 9:cd3607ba5643 192
WD40andTape 10:1b6daba32452 193 threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
WD40andTape 7:5b6a2cefbf3b 194 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
WD40andTape 21:0b10d8e615d1 195 threadSetDemands.start(setDemandsForLL); // Start planning thread
WD40andTape 17:bbaf3e8440ad 196 threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread
WD40andTape 7:5b6a2cefbf3b 197
WD40andTape 21:0b10d8e615d1 198 setDemandsTicker.attach(&startLLcomms, 1/LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 199
WD40andTape 7:5b6a2cefbf3b 200 Thread::wait(1);
WD40andTape 1:2a43cf183a62 201 while(1) {
WD40andTape 1:2a43cf183a62 202 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 203 }
WD40andTape 7:5b6a2cefbf3b 204 }