MorphGI
Mid level control code
Dependencies: ros_lib_kinetic
Diff: main.cpp
- Revision:
- 0:607bc887b6e0
- Child:
- 1:2a43cf183a62
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Jul 25 10:43:23 2018 +0000
@@ -0,0 +1,439 @@
+#include "mbed.h"
+#include "math.h"
+
+//#include "mbed.h"
+#include "mbed_events.h"
+
+//ADC SPI stuff
+#define PREAMBLE 0x06
+#define CHAN_1 0x30
+#define CHAN_2 0x70
+#define CHAN_3 0xB0
+#define CHAN_4 0xF0
+
+#define DATA_MASK 0x0F
+
+#define ADC_PRESSURE 1
+#define ADC_POSITION 3
+
+#define N_CHANNELS 8 //number of channels to control
+ //1-3: front segment
+ //4-6: rear segment
+ //7-8: mid segment
+
+//parameters to manually change
+#define LOW_LEVEL_SPI_FREQUENCY 10000000
+
+#define PATH_SAMPLE_TIME_S 0.05
+
+#define MAX_LENGTH_MM 100.0
+#define MAX_ACTUATOR_LENGTH 50.0
+#define MAX_SPEED_MMPS 24.3457
+#define PATH_TOLERANCE_MM 0.2
+
+double dblMaxChamberLengths_mm[N_CHANNELS] = {100.0,50.0,50.0,50.0,50.0,50.0,30.0,30.0};
+
+
+
+int ii,jj,kk; //counting varaibles
+
+//-----These are the variables being shared between High-Level and Mid-Level-----------------------------------------------//
+
+double dblPSI[3][3]; //the message from high-level containing the chamber lengths in meters in following format:
+ /*
+ {L(front,1), L(front,2), L(front,3);
+ L(mid,1) , L(mid,2) , L(mid,3);
+ L(rear,1) , L(rear,2) , L(rear,3);}
+ */
+
+double dblPosition_mtrs[N_CHANNELS]; //the actual chamber lengths in meters given as the change in length relative to neutral (should always be >=0)
+double dblPressure_bar[N_CHANNELS]; //the pressure in a given chamber in bar (1 bar = 100,000 Pa).
+double dblTargetTime_s; //the time in seconds desired to reach the target (>0...!)
+
+//
+Semaphore semReplan(1);// this must be set every time new high-level transmissions are received to allow replanning to take place!
+//--------------------------------------------------------------------------------------------------------------------------//
+
+//boolean flag to indicate that new target information has been received
+//bool blnReplan;//set this when new transmission is received over ethernet
+
+//path variables
+double dblVelocity_mmps[N_CHANNELS];//the linear path velocity (not sent to actuator)
+double dblLinearPath_mm[N_CHANNELS]; //the current position of the linear path (not sent to actuator)
+double dblSmoothPath_mm[N_CHANNELS]; //the current position of the smooth path (not sent to actuator)
+double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
+double dblTargetActLen_mm[N_CHANNELS];
+double dblPathToActuator[N_CHANNELS];//the target position for the actuator (sent to actuator)
+
+int intDemPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator
+int intPosSPI_Rx[N_CHANNELS]; //13 bit value received over SPI from the actuator
+int intPosADC_Rx[N_CHANNELS]; //12-bit ADC reading of potentiometer on actuator
+
+double dblPathTolerance; //how close the linear path must get to the target position before considering it a success.
+
+double dblActuatorConversion[N_CHANNELS] = {0.24176,0.24176,0.24176,0.24176,0.24176,0.36264,0.36264};
+
+double dblSmoothingFactor = 0.5;
+
+char chrErrorFlag[N_CHANNELS];
+
+Serial pc(USBTX, USBRX); // tx, rx for usb debugging
+
+SPI spi(PC_12,PC_11, PC_10); // mosi, miso, sclk
+
+DigitalOut cs_LL[N_CHANNELS] = {PF_10, PD_13, PE_7, PD_12, PD_14, PD_11, PD_15, PE_10};//chip select for low level controller
+DigitalOut cs_ADC[N_CHANNELS] = {PF_1, PF_0, PD_1, PD_0, PG_0, PE_1, PG_9, PG_12}; //chip select for ADC
+
+InterruptIn pinGate[N_CHANNELS] ={PE_11, PE_13, PF_3, PF_13, PF_15, PF_12, PF_11, PG_14};//gate interrupt pins
+
+DigitalOut pinReset(PD_2); //reset pin for all controllers.
+
+EventQueue queue(32 * EVENTS_EVENT_SIZE);
+
+Thread t(osPriorityRealtime);
+
+Thread threadReplan(osPriorityBelowNormal);
+Thread threadPathPlan(osPriorityNormal);
+
+Thread threadSimulateDemand(osPriorityHigh);
+
+Mutex mut[N_CHANNELS];
+
+Semaphore semPathPlan(1);//
+
+Timer timer;//timers are nice
+
+
+/*
+ unsigned int result1 = 0;
+ unsigned int result2 = 0;
+ unsigned int result3 = 0;
+ unsigned int result4 = 0;
+
+ unsigned int result1a = 0;
+ unsigned int result1b = 0;
+*/
+
+
+
+
+double dblDemPosition[N_CHANNELS];
+double dblPosition[N_CHANNELS];
+double dblPressure[N_CHANNELS];
+
+int ThreadID[N_CHANNELS];
+
+bool blnDataReady[N_CHANNELS];
+
+unsigned int ReadADCPosition(int channel)
+{
+ unsigned int outputA;
+ unsigned int outputB;
+ unsigned int output;
+
+ spi.format(8,0);
+ spi.frequency(1000000);
+
+ cs_ADC[channel] = 0;
+ spi.write(PREAMBLE);
+ outputA = spi.write(CHAN_3);
+ outputB = spi.write(0xFF);
+ cs_ADC[channel] = 1;
+
+ outputA = outputA & DATA_MASK;
+ outputA = outputA<<8;
+ output = (outputA | outputB);
+
+ return output;
+}
+
+unsigned int ReadADCPressure(int channel)
+{
+ unsigned int outputA;
+ unsigned int outputB;
+ unsigned int output;
+
+ spi.format(8,0);
+ spi.frequency(1000000);
+
+ cs_ADC[channel] = 0;
+ spi.write(PREAMBLE);
+ outputA = spi.write(CHAN_1);
+ outputB = spi.write(0xFF);
+ cs_ADC[channel] = 1;
+
+ outputA = outputA & DATA_MASK;
+ outputA = outputA<<8;
+ output = (outputA | outputB);
+
+ return output;
+}
+
+void TransmitData(int channel)
+{
+ //get data from controller
+ spi.format(16,2);
+ spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
+
+ cs_LL[channel] = 0;//select relevant chip
+ intPosSPI_Rx[channel] = spi.write(intDemPos_Tx[channel]); //transmit & receive
+ cs_LL[channel] = 1;//deselect chip
+
+ //sort out received data
+ chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
+ intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
+ dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*MAX_ACTUATOR_LENGTH/dblActuatorConversion[channel];
+}
+
+//common rise handler function
+
+void common_rise_handler(int channel)
+{
+ //check if data is ready for tranmission
+ if (blnDataReady[channel])
+ {
+ // Add transmit task to event queue
+ blnDataReady[channel] = 0;//data no longer ready
+ ThreadID[channel] = queue.call(TransmitData,channel);//schedule transmission
+ }
+}
+
+
+
+//common_fall handler functions
+void common_fall_handler(int channel)
+{
+ //cancel relevant queued event
+ queue.cancel(ThreadID[channel]);
+}
+
+//stub rise functions
+void rise0(void) { common_rise_handler(0); }
+void rise1(void) { common_rise_handler(1); }
+void rise2(void) { common_rise_handler(2); }
+void rise3(void) { common_rise_handler(3); }
+void rise4(void) { common_rise_handler(4); }
+void rise5(void) { common_rise_handler(5); }
+void rise6(void) { common_rise_handler(6); }
+void rise7(void) { common_rise_handler(7); }
+
+//stub fall functions
+void fall0(void) { common_fall_handler(0); }
+void fall1(void) { common_fall_handler(1); }
+void fall2(void) { common_fall_handler(2); }
+void fall3(void) { common_fall_handler(3); }
+void fall4(void) { common_fall_handler(4); }
+void fall5(void) { common_fall_handler(5); }
+void fall6(void) { common_fall_handler(6); }
+void fall7(void) { common_fall_handler(7); }
+
+void startPathPlan()
+{
+ semPathPlan.release();
+}
+
+//this function will be called when a new transmission is received from high level
+void ReplanPath()
+{
+ //while(1)
+ //{
+ //semReplan.wait();//wait until called
+ //printf("replan!\r\n");
+ for (ii = 0; ii < N_CHANNELS; ii++)
+ {
+ mut[ii].lock();
+ }
+
+ //update front segment
+ dblTargetChambLen_mm[0] = dblPSI[0][0]*1000;
+ dblTargetChambLen_mm[1] = dblPSI[0][1]*1000;
+ dblTargetChambLen_mm[2] = dblPSI[0][2]*1000;
+ //update mid segment
+ dblTargetChambLen_mm[6] = dblPSI[1][0]*1000;
+ dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; //same because two pumps are used
+ //update rear segment
+ dblTargetChambLen_mm[3] = dblPSI[2][0]*1000;
+ dblTargetChambLen_mm[4] = dblPSI[2][1]*1000;
+ dblTargetChambLen_mm[5] = dblPSI[2][2]*1000;
+
+ for (ii = 0; ii < N_CHANNELS; ii++)
+ {
+ mut[ii].unlock();
+ }
+
+ for (ii = 0; ii< N_CHANNELS; ii++)
+ {
+ mut[ii].lock();//lock variables to avoid race condition
+
+ //check to see if positions are achievable
+ if(dblTargetChambLen_mm[ii]>dblMaxChamberLengths_mm[ii])
+ {
+ dblTargetChambLen_mm[ii] = dblMaxChamberLengths_mm[ii];
+ }
+
+ if(dblTargetChambLen_mm[ii]<0.0)
+ {
+ dblTargetChambLen_mm[ii] = 0.0;
+ }
+
+ dblTargetActLen_mm[ii] = dblTargetChambLen_mm[ii]*dblActuatorConversion[ii];
+
+ //work out new velocities
+ dblVelocity_mmps[ii] = (dblTargetActLen_mm[ii] - dblLinearPath_mm[ii]) / dblTargetTime_s;
+
+ //check to see if velocities are achievable
+ if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS)
+ {
+ if(dblVelocity_mmps[ii]>0)
+ {
+ dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
+ }
+ else
+ {
+ dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
+ }
+ }
+
+ mut[ii].unlock(); //unlock mutex.
+ }
+ //}
+
+
+}
+
+void CalculatePath()
+{
+ while(1)
+ {
+ semPathPlan.wait();
+ //if(blnReplan)
+// {
+// blnReplan = 0;//remove flag
+// ReplanPath(dblPSI, dblTargetTime_s);
+// }
+ for(ii = 0; ii < N_CHANNELS; ii++)
+ {
+ //calculate next step in linear path
+ mut[ii].lock();//lock relevant mutex
+ dblLinearPath_mm[ii] = dblLinearPath_mm[ii] + dblVelocity_mmps[ii]*PATH_SAMPLE_TIME_S;
+ //check tolerance
+ if (abs(dblLinearPath_mm[ii] - dblTargetActLen_mm[ii]) <= dblPathTolerance)
+ {
+ dblVelocity_mmps[ii] = 0; //stop linear path generation when linear path is within tolerance of target position.
+ }
+ mut[ii].unlock();//unlock relevant mutex
+
+ //calculate next step in smooth path
+ dblSmoothPath_mm[ii] = dblSmoothingFactor*dblLinearPath_mm[ii] + (1.0-dblSmoothingFactor)*dblSmoothPath_mm[ii];
+
+ //convert to actuator distances
+ dblPathToActuator[ii] = dblSmoothPath_mm[ii];
+
+ intDemPos_Tx[ii] = (int) dblPathToActuator[ii]/MAX_ACTUATOR_LENGTH*8191;//convert to a 13-bit number;
+ intDemPos_Tx[ii] = intDemPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit
+
+
+ blnDataReady[ii] = 1;//signal that data ready
+
+ }
+ printf("%d, %f,%f,%f, %f\r\n",timer.read_ms(), dblTargetActLen_mm[0] ,dblVelocity_mmps[0], dblLinearPath_mm[0], dblSmoothPath_mm[0]);
+ }
+}
+
+void SimulateDemand()
+{
+ while(1)
+ {
+ mut[0].lock();
+ if(kk == 0)
+ {
+ dblPSI[0][0] = (double) 10.0;
+ dblTargetTime_s = 1.0;
+ }
+ else
+ {
+ dblPSI[0][0] = (double) 0.0;
+ dblTargetTime_s = 2.0;
+ }
+
+ kk = 1 - kk;
+
+
+ //semReplan.release();
+
+ mut[0].unlock();
+ ReplanPath();
+
+ Thread::wait(7000);
+ }
+}
+
+
+
+Ticker PathCalculationTicker;
+
+int main()
+{
+ //initialise relevant variables
+ for(ii = 0; ii<N_CHANNELS; ii++)
+ {
+ //all chip selects in off state
+ cs_LL[ii] = 1;
+ cs_ADC[ii] = 1;
+
+ //data ready flags set to not ready
+ blnDataReady[ii] = 0;
+ }
+
+ //calculate some control variables
+ dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance.
+
+ pinReset = 1; //initialise reset pin to not reset the controllers.
+
+ //say something nice to the user.
+ pc.baud(9600);
+ printf("Hi, there! I'll be your mid-level controller for today.\r\n");
+
+ // Start the event queue
+ t.start(callback(&queue, &EventQueue::dispatch_forever));
+
+ //set up the interrupts
+
+ //set up rise interrupts MIGHT NOT NEED TO BE POINTERS
+ pinGate[0].rise(&rise0);
+ pinGate[1].rise(&rise1);
+ pinGate[2].rise(&rise2);
+ pinGate[3].rise(&rise3);
+ pinGate[4].rise(&rise4);
+ pinGate[5].rise(&rise5);
+ pinGate[6].rise(&rise6);
+ pinGate[7].rise(&rise7);
+
+ //set up fall interrupts MIGHT NOT NEED TO BE POINTERS
+ pinGate[0].fall(&fall0);
+ pinGate[1].fall(&fall1);
+ pinGate[2].fall(&fall2);
+ pinGate[3].fall(&fall3);
+ pinGate[4].fall(&fall4);
+ pinGate[5].fall(&fall5);
+ pinGate[6].fall(&fall6);
+ pinGate[7].fall(&fall7);
+
+ timer.start();
+ kk = 0;
+
+ threadSimulateDemand.start(SimulateDemand);
+ threadPathPlan.start(CalculatePath); //start planning thread
+ PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); //set up planning thread to recur at fixed intervals
+ threadReplan.start(ReplanPath);//start Replanning thread
+ Thread::wait(1);
+
+
+ while(1) {
+
+
+
+ Thread::wait(osWaitForever);
+ }
+}
+