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@32:8c59c536a2a6, 2019-03-08 (annotated)

Committer:
WD40andTape
Date:
Fri Mar 08 12:44:29 2019 +0000
Revision:
32:8c59c536a2a6
Parent:
31:08cb04eb75fc
Child:
33:9877ca32e43c
Fixed memory overflow bug in planning for separate segments.

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