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@29:10a5cf37a875, 2019-02-08 (annotated)

Committer:
WD40andTape
Date:
Fri Feb 08 18:36:15 2019 +0000
Revision:
29:10a5cf37a875
Parent:
28:8e0c502c1a50
Child:
31:08cb04eb75fc
Actuator numbering standardised. Units converted to mm. Added text message feedback. Using Int16MultiArray instead of Float32MultiArray.

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 21:0b10d8e615d1 11 // DEMAND VARIABLES
WD40andTape 26:7c59002c9cd7 12 double dblDemandSpeed_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
WD40andTape 26:7c59002c9cd7 13 double dblDemandPosition_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
dofydoink 11:7029367a1840 14
WD40andTape 7:5b6a2cefbf3b 15 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
dofydoink 12:595ed862e52f 16 LLComms llcomms;
WD40andTape 28:8e0c502c1a50 17 HLComms hlcomms(HL_COMMS_FREQ_HZ);
WD40andTape 3:c83291bf9fd2 18
WD40andTape 10:1b6daba32452 19 Thread threadLowLevelSPI(osPriorityRealtime);
WD40andTape 21:0b10d8e615d1 20 Thread threadSetDemands(osPriorityNormal);
WD40andTape 4:303584310071 21 Thread threadReceiveAndReplan(osPriorityBelowNormal);
WD40andTape 17:bbaf3e8440ad 22 Thread threadSensorFeedback(osPriorityBelowNormal);
dofydoink 0:607bc887b6e0 23
WD40andTape 4:303584310071 24 Mutex mutPathIn;
WD40andTape 21:0b10d8e615d1 25 Semaphore semLLcomms(1);
WD40andTape 17:bbaf3e8440ad 26 Semaphore semSensorData(1);
dofydoink 0:607bc887b6e0 27
WD40andTape 21:0b10d8e615d1 28 Ticker setDemandsTicker;
WD40andTape 17:bbaf3e8440ad 29 Ticker SendSensorDataTicker;
WD40andTape 17:bbaf3e8440ad 30
WD40andTape 17:bbaf3e8440ad 31 void sendSensorData() {
WD40andTape 17:bbaf3e8440ad 32 while( true ) {
WD40andTape 17:bbaf3e8440ad 33 semSensorData.wait();
WD40andTape 29:10a5cf37a875 34 hlcomms.send_sensor_message(llcomms.positionSensor_uint,llcomms.pressureSensor_uint);
WD40andTape 17:bbaf3e8440ad 35 }
WD40andTape 17:bbaf3e8440ad 36 }
WD40andTape 17:bbaf3e8440ad 37
WD40andTape 17:bbaf3e8440ad 38 void signalSendSensorData() {
WD40andTape 17:bbaf3e8440ad 39 semSensorData.release();
WD40andTape 17:bbaf3e8440ad 40 }
dofydoink 0:607bc887b6e0 41
WD40andTape 7:5b6a2cefbf3b 42 // This function will be called when a new transmission is received from high level
WD40andTape 7:5b6a2cefbf3b 43 void ReceiveAndReplan() {
WD40andTape 6:f0a18e28a322 44
WD40andTape 22:82871f00f89d 45 SendSensorDataTicker.attach(&signalSendSensorData, 1/(float)SENSOR_FEEDBACK_HZ); // Set up planning thread to recur at fixed intervals
WD40andTape 17:bbaf3e8440ad 46
WD40andTape 28:8e0c502c1a50 47 struct demands_struct input;
WD40andTape 4:303584310071 48
WD40andTape 6:f0a18e28a322 49 while( true ) {
WD40andTape 28:8e0c502c1a50 50 hlcomms.newData.wait();
WD40andTape 28:8e0c502c1a50 51 input = hlcomms.get_demands();
WD40andTape 1:2a43cf183a62 52
WD40andTape 7:5b6a2cefbf3b 53 // PROCESS INPUT
WD40andTape 14:54c3759e76ed 54
WD40andTape 28:8e0c502c1a50 55 /*input.psi_mm[5] = -1; // Disable additional mid actuator
WD40andTape 28:8e0c502c1a50 56 input.psi_mm[7] = -1; // Disable additional rear actuator
WD40andTape 28:8e0c502c1a50 57 input.psi_mm[8] = -1; // Disable additional rear actuator*/
dofydoink 11:7029367a1840 58
WD40andTape 19:e5acb2183d4e 59 // Limit requested speed
WD40andTape 29:10a5cf37a875 60 double maxTimeToTarget_s;
WD40andTape 28:8e0c502c1a50 61 double limitedSpeed_mmps = min( max( 0.0 , input.speed_mmps ) , (double)MAX_SPEED_MMPS );
WD40andTape 29:10a5cf37a875 62 if( limitedSpeed_mmps > 0 ) {
WD40andTape 29:10a5cf37a875 63 // Lock mutex, preventing setDemandsForLL from running
WD40andTape 29:10a5cf37a875 64 mutPathIn.lock();
WD40andTape 29:10a5cf37a875 65 // For each actuator, limit the input position, calculate the position change, and select the absolute max
WD40andTape 29:10a5cf37a875 66 double dblDistanceToTarget_mm[N_CHANNELS] = { -1.0 };
WD40andTape 29:10a5cf37a875 67 double maxDistanceToTarget_mm = 0.0;
WD40andTape 29:10a5cf37a875 68 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 29:10a5cf37a875 69 double dblCurrentPosition_mm = llcomms.positionSensor_mm[i];
WD40andTape 29:10a5cf37a875 70 if(input.psi_mm[i]<0 || dblCurrentPosition_mm<0) { // If requested position is negative or the sensor feedback is erroneous
WD40andTape 29:10a5cf37a875 71 continue;
WD40andTape 29:10a5cf37a875 72 }
WD40andTape 19:e5acb2183d4e 73 // Limit actuator position
WD40andTape 29:10a5cf37a875 74 input.psi_mm[i] = min( max( 0.0 , input.psi_mm[i] ) , (double)MAX_ACTUATOR_LIMIT_MM );
WD40andTape 19:e5acb2183d4e 75 // Calculate actuator position change
WD40andTape 29:10a5cf37a875 76 dblDistanceToTarget_mm[i] = fabs(input.psi_mm[i] - dblCurrentPosition_mm);
WD40andTape 19:e5acb2183d4e 77 // Select the max absolute actuator position change
WD40andTape 29:10a5cf37a875 78 if(dblDistanceToTarget_mm[i]>maxDistanceToTarget_mm) {
WD40andTape 29:10a5cf37a875 79 maxDistanceToTarget_mm = dblDistanceToTarget_mm[i];
WD40andTape 19:e5acb2183d4e 80 }
WD40andTape 19:e5acb2183d4e 81 }
WD40andTape 29:10a5cf37a875 82 // For max actuator position change, calculate the time to destination at the limited speed
WD40andTape 29:10a5cf37a875 83 maxTimeToTarget_s = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
WD40andTape 29:10a5cf37a875 84 // For each actuator, replan target position and velocity as required
WD40andTape 29:10a5cf37a875 85 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 29:10a5cf37a875 86 // If requested actuator position change is already within tolerance, do NOT replan that actuator
WD40andTape 29:10a5cf37a875 87 if( dblDistanceToTarget_mm[i] <= 0.0 ) continue;
WD40andTape 29:10a5cf37a875 88 // Calculate velocity for each motor to synchronise movements to complete in max time
WD40andTape 29:10a5cf37a875 89 // Set dblDemandPosition_mm and dblDemandSpeed_mmps
WD40andTape 29:10a5cf37a875 90 dblDemandPosition_mm[i] = input.psi_mm[i];
WD40andTape 29:10a5cf37a875 91 dblDemandSpeed_mmps[i] = dblDistanceToTarget_mm[i] / maxTimeToTarget_s;
WD40andTape 29:10a5cf37a875 92 }
WD40andTape 29:10a5cf37a875 93 // Unlock mutex, allowing setDemandsForLL to run again
WD40andTape 29:10a5cf37a875 94 mutPathIn.unlock();
WD40andTape 29:10a5cf37a875 95 } else {
WD40andTape 29:10a5cf37a875 96 maxTimeToTarget_s = -1;
dofydoink 11:7029367a1840 97 }
dofydoink 12:595ed862e52f 98
WD40andTape 6:f0a18e28a322 99 // SEND MESSAGE
WD40andTape 28:8e0c502c1a50 100 hlcomms.send_duration_message(maxTimeToTarget_s);
WD40andTape 6:f0a18e28a322 101 }
WD40andTape 6:f0a18e28a322 102
dofydoink 0:607bc887b6e0 103 }
dofydoink 0:607bc887b6e0 104
WD40andTape 24:bc852aa89e7a 105 void startLLcomms() { // Send new demands to LL after receiving new target data
WD40andTape 24:bc852aa89e7a 106 semLLcomms.release(); // Uses threadSetDemands which is normal priority
WD40andTape 24:bc852aa89e7a 107 }
WD40andTape 24:bc852aa89e7a 108
WD40andTape 21:0b10d8e615d1 109 void setDemandsForLL() {
WD40andTape 21:0b10d8e615d1 110
WD40andTape 4:303584310071 111 while(1) {
WD40andTape 21:0b10d8e615d1 112 semLLcomms.wait();
WD40andTape 21:0b10d8e615d1 113 mutPathIn.lock(); // Lock relevant mutex
WD40andTape 21:0b10d8e615d1 114 for(short int i=0; i<N_CHANNELS; i++) { // For each LL
WD40andTape 21:0b10d8e615d1 115 llcomms.mutChannel[i].lock(); // MUTEX LOCK
WD40andTape 26:7c59002c9cd7 116 llcomms.demandPosition_mm[i] = dblDemandPosition_mm[i];
WD40andTape 26:7c59002c9cd7 117 llcomms.demandSpeed_mmps[i] = dblDemandSpeed_mmps[i];
WD40andTape 21:0b10d8e615d1 118 llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
WD40andTape 22:82871f00f89d 119 llcomms.isDataReady[i] = 1; // Signal that data ready
WD40andTape 21:0b10d8e615d1 120 } // end for
WD40andTape 21:0b10d8e615d1 121 mutPathIn.unlock(); // Unlock relevant mutex
WD40andTape 24:bc852aa89e7a 122 } // end while(1)
WD40andTape 26:7c59002c9cd7 123
WD40andTape 3:c83291bf9fd2 124 }
dofydoink 0:607bc887b6e0 125
WD40andTape 13:a373dfc57b89 126 int main() {
WD40andTape 7:5b6a2cefbf3b 127 pc.baud(BAUD_RATE);
WD40andTape 17:bbaf3e8440ad 128 printf("ML engage. Compiled at %s\r\n.",__TIME__);
dofydoink 5:712e7634c779 129 wait(3);
WD40andTape 9:cd3607ba5643 130
WD40andTape 10:1b6daba32452 131 threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
WD40andTape 7:5b6a2cefbf3b 132 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
WD40andTape 21:0b10d8e615d1 133 threadSetDemands.start(setDemandsForLL); // Start planning thread
WD40andTape 17:bbaf3e8440ad 134 threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread
WD40andTape 7:5b6a2cefbf3b 135
WD40andTape 24:bc852aa89e7a 136 setDemandsTicker.attach(&startLLcomms, 1/(float)LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 137
WD40andTape 7:5b6a2cefbf3b 138 Thread::wait(1);
WD40andTape 1:2a43cf183a62 139 while(1) {
WD40andTape 1:2a43cf183a62 140 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 141 }
WD40andTape 7:5b6a2cefbf3b 142 }