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@31:08cb04eb75fc, 2019-03-06 (annotated)

Committer:
WD40andTape
Date:
Wed Mar 06 10:28:59 2019 +0000
Revision:
31:08cb04eb75fc
Parent:
29:10a5cf37a875
Child:
32:8c59c536a2a6
Updated to accept input speed, process, and output time separately for each segment. Tested and working.

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 31:08cb04eb75fc 54 double maxTimesToTarget_s[3];
WD40andTape 31:08cb04eb75fc 55 for(short int segNum=0; segNum<3; segNum++) {
WD40andTape 31:08cb04eb75fc 56 // Limit requested speed
WD40andTape 31:08cb04eb75fc 57 double limitedSpeed_mmps = min( max( 0.0 , input.speeds_mmps[segNum] ) , (double)MAX_SPEED_MMPS );
WD40andTape 31:08cb04eb75fc 58 if( limitedSpeed_mmps > 0 ) {
WD40andTape 31:08cb04eb75fc 59 // Lock mutex, preventing setDemandsForLL from running
WD40andTape 31:08cb04eb75fc 60 mutPathIn.lock();
WD40andTape 31:08cb04eb75fc 61 // For each actuator, limit the input position, calculate the position change, and select the absolute max
WD40andTape 31:08cb04eb75fc 62 double dblDistanceToTarget_mm[3] = { -1.0 };
WD40andTape 31:08cb04eb75fc 63 double maxDistanceToTarget_mm = 0.0;
WD40andTape 31:08cb04eb75fc 64 for(int i=segNum*3; i<segNum*3+3; i++) {
WD40andTape 31:08cb04eb75fc 65 double dblCurrentPosition_mm = llcomms.positionSensor_mm[i];
WD40andTape 31:08cb04eb75fc 66 if(input.psi_mm[i]<0 || dblCurrentPosition_mm<0) { // If requested position is negative or the sensor feedback is erroneous
WD40andTape 31:08cb04eb75fc 67 continue;
WD40andTape 31:08cb04eb75fc 68 }
WD40andTape 31:08cb04eb75fc 69 // Limit actuator position
WD40andTape 31:08cb04eb75fc 70 input.psi_mm[i] = min( max( 0.0 , input.psi_mm[i] ) , (double)MAX_ACTUATOR_LIMIT_MM );
WD40andTape 31:08cb04eb75fc 71 // Calculate actuator position change
WD40andTape 31:08cb04eb75fc 72 dblDistanceToTarget_mm[i] = fabs(input.psi_mm[i] - dblCurrentPosition_mm);
WD40andTape 31:08cb04eb75fc 73 // Select the max absolute actuator position change
WD40andTape 31:08cb04eb75fc 74 if(dblDistanceToTarget_mm[i]>maxDistanceToTarget_mm) {
WD40andTape 31:08cb04eb75fc 75 maxDistanceToTarget_mm = dblDistanceToTarget_mm[i];
WD40andTape 31:08cb04eb75fc 76 }
WD40andTape 29:10a5cf37a875 77 }
WD40andTape 31:08cb04eb75fc 78 // For max actuator position change, calculate the time to destination at the limited speed
WD40andTape 31:08cb04eb75fc 79 maxTimesToTarget_s[segNum] = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
WD40andTape 31:08cb04eb75fc 80 // For each actuator, replan target position and velocity as required
WD40andTape 31:08cb04eb75fc 81 for(int i=segNum*3; i<segNum*3+3; i++) {
WD40andTape 31:08cb04eb75fc 82 // If requested actuator position change is already within tolerance, do NOT replan that actuator
WD40andTape 31:08cb04eb75fc 83 if( dblDistanceToTarget_mm[i] <= 0.0 ) continue;
WD40andTape 31:08cb04eb75fc 84 // Calculate velocity for each motor to synchronise movements to complete in max time
WD40andTape 31:08cb04eb75fc 85 // Set dblDemandPosition_mm and dblDemandSpeed_mmps
WD40andTape 31:08cb04eb75fc 86 dblDemandPosition_mm[i] = input.psi_mm[i];
WD40andTape 31:08cb04eb75fc 87 dblDemandSpeed_mmps[i] = dblDistanceToTarget_mm[i] / maxTimesToTarget_s[segNum];
WD40andTape 19:e5acb2183d4e 88 }
WD40andTape 31:08cb04eb75fc 89 // Unlock mutex, allowing setDemandsForLL to run again
WD40andTape 31:08cb04eb75fc 90 mutPathIn.unlock();
WD40andTape 31:08cb04eb75fc 91 } else {
WD40andTape 31:08cb04eb75fc 92 maxTimesToTarget_s[segNum] = -1;
WD40andTape 19:e5acb2183d4e 93 }
dofydoink 11:7029367a1840 94 }
dofydoink 12:595ed862e52f 95
WD40andTape 6:f0a18e28a322 96 // SEND MESSAGE
WD40andTape 31:08cb04eb75fc 97 hlcomms.send_durations_message(maxTimesToTarget_s);
WD40andTape 6:f0a18e28a322 98 }
WD40andTape 6:f0a18e28a322 99
dofydoink 0:607bc887b6e0 100 }
dofydoink 0:607bc887b6e0 101
WD40andTape 24:bc852aa89e7a 102 void startLLcomms() { // Send new demands to LL after receiving new target data
WD40andTape 24:bc852aa89e7a 103 semLLcomms.release(); // Uses threadSetDemands which is normal priority
WD40andTape 24:bc852aa89e7a 104 }
WD40andTape 24:bc852aa89e7a 105
WD40andTape 21:0b10d8e615d1 106 void setDemandsForLL() {
WD40andTape 21:0b10d8e615d1 107
WD40andTape 4:303584310071 108 while(1) {
WD40andTape 21:0b10d8e615d1 109 semLLcomms.wait();
WD40andTape 21:0b10d8e615d1 110 mutPathIn.lock(); // Lock relevant mutex
WD40andTape 21:0b10d8e615d1 111 for(short int i=0; i<N_CHANNELS; i++) { // For each LL
WD40andTape 21:0b10d8e615d1 112 llcomms.mutChannel[i].lock(); // MUTEX LOCK
WD40andTape 26:7c59002c9cd7 113 llcomms.demandPosition_mm[i] = dblDemandPosition_mm[i];
WD40andTape 26:7c59002c9cd7 114 llcomms.demandSpeed_mmps[i] = dblDemandSpeed_mmps[i];
WD40andTape 21:0b10d8e615d1 115 llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
WD40andTape 22:82871f00f89d 116 llcomms.isDataReady[i] = 1; // Signal that data ready
WD40andTape 21:0b10d8e615d1 117 } // end for
WD40andTape 21:0b10d8e615d1 118 mutPathIn.unlock(); // Unlock relevant mutex
WD40andTape 24:bc852aa89e7a 119 } // end while(1)
WD40andTape 26:7c59002c9cd7 120
WD40andTape 3:c83291bf9fd2 121 }
dofydoink 0:607bc887b6e0 122
WD40andTape 13:a373dfc57b89 123 int main() {
WD40andTape 7:5b6a2cefbf3b 124 pc.baud(BAUD_RATE);
WD40andTape 31:08cb04eb75fc 125 //printf("ML engage. Compiled at %s\r\n.",__TIME__);
dofydoink 5:712e7634c779 126 wait(3);
WD40andTape 9:cd3607ba5643 127
WD40andTape 10:1b6daba32452 128 threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
WD40andTape 7:5b6a2cefbf3b 129 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
WD40andTape 21:0b10d8e615d1 130 threadSetDemands.start(setDemandsForLL); // Start planning thread
WD40andTape 17:bbaf3e8440ad 131 threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread
WD40andTape 7:5b6a2cefbf3b 132
WD40andTape 24:bc852aa89e7a 133 setDemandsTicker.attach(&startLLcomms, 1/(float)LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 134
WD40andTape 7:5b6a2cefbf3b 135 Thread::wait(1);
WD40andTape 1:2a43cf183a62 136 while(1) {
WD40andTape 1:2a43cf183a62 137 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 138 }
WD40andTape 7:5b6a2cefbf3b 139 }