MorphGI
Mid level control code
Dependencies: ros_lib_kinetic
Diff: main.cpp
- Revision:
- 4:303584310071
- Parent:
- 3:c83291bf9fd2
- Child:
- 5:712e7634c779
--- a/main.cpp Tue Jul 31 14:30:40 2018 +0000
+++ b/main.cpp Tue Jul 31 17:54:29 2018 +0000
@@ -26,31 +26,18 @@
//parameters to manually change
#define LOW_LEVEL_SPI_FREQUENCY 10000000
-#define PATH_SAMPLE_TIME_S 0.05
+#define PATH_SAMPLE_TIME_S 0.005
#define MAX_LENGTH_MM 100.0
#define MAX_ACTUATOR_LENGTH 52.2
#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; //counting varaibles
-
-//-----These are the variables being shared between High-Level and Mid-Level-----------------------------------------------//
+#define PATH_TOLERANCE_MM 0.2 //how close the linear path must get to the target position before considering it a success.
-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...!)
+const double DBL_MAX_CHAMBER_LENGTHS_MM[N_CHANNELS] = {100.0,50.0,50.0,50.0,50.0,50.0,30.0,30.0};
+const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {0.24176,0.24176,0.24176,0.24176,0.24176,0.36264,0.36264};// covert from chamber lengths to actuator lengths
+const double DBL_SMOOTHING_FACTOR = 0.5; // 0<x<=1, where 1 is no smoothing
+const double DBL_PATH_TOLERANCE = 0.2;
+//-----These are the variables being shared between High-Level and Mid-Level-----------------------------------------------//
//
Semaphore semReplan(1);// this must be set every time new high-level transmissions are received to allow replanning to take place!
@@ -60,28 +47,17 @@
//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 dblLinearPathCurrentPos_mm[N_CHANNELS]; //the current position of the linear path (not sent to actuator)
-double dblSmoothPathCurrentPos_mm[N_CHANNELS]; //the current position of the smooth path (not sent to actuator)
-
-double dblTargetActLen_mm[N_CHANNELS];
-double dblPathToActuator[N_CHANNELS];//the target position for the actuator (sent to actuator)
-
-int intDemandPos_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 dblVelocity_mmps[N_CHANNELS] = { 0.0 };//the linear path velocity (not sent to actuator) KEEP GLOBAL
+double dblLinearPathCurrentPos_mm[N_CHANNELS]={ 0.0 }; //the current position of the linear path (not sent to actuator) //KEEP GLOBAL
+double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; //The final target position for the actuator KEEP GLOBAL
-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};// covert from chamber lengths to actuator lengths
+int intDemandPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator KEEP GLOBAL
-double dblSmoothingFactor = 0.5; // 0<x<=1, where 1 is no smoothing
+char chrErrorFlag[N_CHANNELS];// 3 error bits from LL KEEP GLOBAL
-char chrErrorFlag[N_CHANNELS];// 3 error bits from LL
-
+//pin declarations----------------
Serial pc(USBTX, USBRX); // tx, rx for usb debugging
-
InterruptIn pinGate6(PE_11); //this pin HAS TO BE defined before SPI set up. No Clue Why.
SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
@@ -91,13 +67,6 @@
DigitalOut pinCheck(PE_5);
-//InterruptIn pinGate[N_CHANNELS] ={PF_11, PG_14, PF_15, PF_12, PF_3, PF_13, PE_11, PE_13};//gate interrupt pins
-
-//WRONG!!!!!!!!!!
-//InterruptIn pinGate[N_CHANNELS] ={PG_11, PG_14, PF_15, PF_12, PF_3, PF_13, PE_11, PE_13};//gate interrupt pins
-//WRONG!!!!!
-
-//InterruptIn testPinGate(PF_11);
//these interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
InterruptIn pinGate0(PF_11);
InterruptIn pinGate1(PG_14);
@@ -105,7 +74,7 @@
InterruptIn pinGate3(PF_12);
InterruptIn pinGate4(PF_3);
InterruptIn pinGate5(PF_13);
-//InterruptIn pinGate6(PE_11); //
+//InterruptIn pinGate6(PE_11); //see above nonsense
InterruptIn pinGate7(PE_13);
DigitalOut pinReset(PD_2); //reset pin for all controllers.
@@ -115,42 +84,24 @@
Thread t(osPriorityRealtime);
-Thread threadReplan(osPriorityBelowNormal);
-Thread threadPathPlan(osPriorityNormal);
+Thread threadReceiveAndReplan(osPriorityBelowNormal);
+Thread threadSmoothPathPlan(osPriorityNormal);
Thread threadSimulateDemand(osPriorityHigh);
Mutex mutChannel[N_CHANNELS];
-Mutex mutPsi;
+Mutex mutPathIn;
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 dblDemandPosition[N_CHANNELS];
-double dblPosition[N_CHANNELS];
-double dblPressure[N_CHANNELS];
-*/
-
int ThreadID[N_CHANNELS];
bool isDataReady[N_CHANNELS]; // flag to indicate path data is ready for transmission to low level.
-double ReadADCPosition(int channel)
+
+double ReadADCPosition_mtrs(int channel)
{
unsigned int outputA;
unsigned int outputB;
@@ -170,11 +121,11 @@
outputA = outputA<<8;
output = (outputA | outputB);
output = 4095- output;
- dblOutput = (double) (output - 1504)/4095*100.0;
+ dblOutput = (double) ((output - 1504)/4095*0.1);
return dblOutput;
}
-double ReadADCPressure(int channel)
+double ReadADCPressure_bar(int channel)
{
unsigned int outputA;
unsigned int outputB;
@@ -194,17 +145,21 @@
outputA = outputA<<8;
output = (outputA | outputB);
- dblOutput = (double) (output-502)/4095*8.0;
+ dblOutput = (double) ((output-502)/4095*8.0);
return dblOutput;
}
void SendReceiveData(int channel)
{
+
+ int intPosSPI_Rx[N_CHANNELS]; //13 bit value received over SPI from the actuator
+
+
//get data from controller
spi.format(16,2);
spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
-
+ mutChannel[channel].lock();//lock mutex for specific Channel
cs_LL[channel] = 0;//select relevant chip
intPosSPI_Rx[channel] = spi.write(intDemandPos_Tx[channel]); //transmit & receive
cs_LL[channel] = 1;//deselect chip
@@ -214,31 +169,15 @@
//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])/1000;
+ //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
+ mutChannel[channel].unlock();//unlock mutex for specific channel
//printf("%d, %d\r\n",intPosSPI_Rx[0],intDemandPos_Tx[0]);
}
-void testSendReceiveData()
-{
- printf("x\r\n");
- //get data from controller
- spi.format(16,2);
- spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
-
- cs_LL[0] = 0;//select relevant chip
- intPosSPI_Rx[0] = spi.write(intDemandPos_Tx[0]); //transmit & receive
- cs_LL[0] = 1;//deselect chip
- isDataReady[0] = 0;//data no longer ready, i.e. we now require new data
-
- //sort out received data
- chrErrorFlag[0] = intPosSPI_Rx[0]>>13;
- intPosSPI_Rx[0] = intPosSPI_Rx[0] & 0x1FFF;
- dblPosition_mtrs[0] = (double)intPosSPI_Rx[0]/8191*(MAX_ACTUATOR_LENGTH/dblActuatorConversion[0])/1000;
-
-
-}
+
//common rise handler function
@@ -248,29 +187,11 @@
//check if data is ready for tranmission
if (isDataReady[channel])
{
-
// Add transmit task to event queue
ThreadID[channel] = queue.call(SendReceiveData,channel);//schedule transmission
}
}
-void testRiseFunction(void)
-{
- pinCheck = 1;
- //check if data is ready for tranmission
- if (isDataReady[0])
- {
- // Add transmit task to event queue
- ThreadID[0] = queue.call(testSendReceiveData);//schedule transmission
- }
-}
-
-void testFallFunction(void)
-{
- pinCheck = 0;
- //cancel relevant queued event
- queue.cancel(ThreadID[0]);
-}
@@ -304,26 +225,43 @@
void startPathPlan() // Plan a new linear path after receiving new target data
{
- semPathPlan.release(); // Uses threadReplan which is below normal priority to ensure consistent transmission to LL
+ semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
}
//this function will be called when a new transmission is received from high level
-void ReplanPath()
+void ReceiveAndReplan()
{
+ int ii;
+
+ 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 dblTargetTime_s; //the time in seconds desired to reach the target (>0...!)
+
+ 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).
+
+ //!!!!!!!!!!!!!!! If received messages faster than path replanning, will get increasingly behind time --> need to throw away some instructions
- //while(1)
- //{
- //semReplan.wait();//wait until called
- //printf("replan!\r\n");
- //for (ii = 0; ii < N_CHANNELS; ii++)
-// {
-// mutChannel[ii].lock();
-// }
+ //receive
+
+ double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
+
+ bool kk = 0;
+ while(1) {
- mutPsi.lock();// lock mutex for PSI to ensure no conflict when receiving new messages while already replanning
- //!!!!!!!!!!!!!!! If received messages faster than path replanning, will get increasingly behind time --> need to throw away some instructions
-
- double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
+ if(kk == 0) {
+ dblPSI[0][0] = (double) 10.0;
+ dblTargetTime_s = 1.5;
+ } else {
+ dblPSI[0][0] = (double) 0.0;
+ dblTargetTime_s = 2.0;
+ }
+ kk = !kk;
+ printf("REPLAN, %f,\r\n", dblPSI[0][0] );
//update front segment
dblTargetChambLen_mm[0] = dblPSI[0][0]*1000;
dblTargetChambLen_mm[1] = dblPSI[0][1]*1000;
@@ -335,55 +273,41 @@
dblTargetChambLen_mm[3] = dblPSI[2][0]*1000;
dblTargetChambLen_mm[4] = dblPSI[2][1]*1000;
dblTargetChambLen_mm[5] = dblPSI[2][2]*1000;
- /* dblTargetChambLen_mm = { psi.
-
- dblTargetTime_s*/
-
- mutPsi.unlock();// unlock mutex for PSI to ensure no conflict when receiving new messages while already replanning
-
-// for (ii = 0; ii < N_CHANNELS; ii++)
-// {
-// mutChannel[ii].unlock();
-// }
- for (ii = 0; ii< N_CHANNELS; ii++)
- {
- mutChannel[ii].lock();//lock variables to avoid race condition
-
+ for (ii = 0; ii< N_CHANNELS; ii++) {
//check to see if positions are achievable
- if(dblTargetChambLen_mm[ii]>dblMaxChamberLengths_mm[ii])
- {
- dblTargetChambLen_mm[ii] = dblMaxChamberLengths_mm[ii];
+ if(dblTargetChambLen_mm[ii]>DBL_MAX_CHAMBER_LENGTHS_MM[ii]) {
+ dblTargetChambLen_mm[ii] = DBL_MAX_CHAMBER_LENGTHS_MM[ii];
}
-
- if(dblTargetChambLen_mm[ii]<0.0)
- {
+ if(dblTargetChambLen_mm[ii]<0.0) {
dblTargetChambLen_mm[ii] = 0.0;
}
- dblTargetActLen_mm[ii] = dblTargetChambLen_mm[ii]*dblActuatorConversion[ii];
-
+ //mutPathIn.lock();//lock variables to avoid race condition
+
+ dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
+
//work out new velocities
- dblVelocity_mmps[ii] = (dblTargetActLen_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblTargetTime_s;
+ dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblTargetTime_s;
//check to see if velocities are achievable
- if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS)
- {
- if(dblVelocity_mmps[ii]>0)
- {
+ if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
+ if(dblVelocity_mmps[ii]>0) {
dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
- }
- else
- {
+ } else {
dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
}
}
+ //mutPathIn.unlock(); //unlock mutex.
- mutChannel[ii].unlock(); //unlock mutex.
- }
- //}
-
-
+ dblPressure_bar[ii] = ReadADCPressure_bar(ii);//read pressure from channel
+ dblPosition_mtrs[ii] = ReadADCPosition_mtrs(ii); //read position from channel
+
+ //send info back to HL
+ } // end of for
+ //printf("%f\r\n", dblVelocity_mmps[0]);
+ Thread::wait(7000);
+ } // end of while(1)
}
// lock mutex
@@ -392,92 +316,54 @@
// do calculations
// lock mutex
// set variables
-// unlock mutex
+// unlock mutex
-void CalculateSmoothPath()
-{
+void CalculateSmoothPath() {
+ int ii;
double dblMeasuredSampleTime;
- while(1)
- {
+ double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; //the current position of the smooth path (not sent to actuator)
+
+ while(1) {
semPathPlan.wait();
- //if(blnReplan)
-// {
-// blnReplan = 0;//remove flag
-// ReplanPath(dblPSI, dblTargetTime_s);
-// }
// If run time is more than 50 us from expected, calculate from measured time step
- if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50)
- {
+ if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50) {
dblMeasuredSampleTime = timer.read();
- }
- else
- {
+ } else {
dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
}
- timer.reset();
+ timer.reset();
- for(ii = 0; ii < N_CHANNELS; ii++)
- {
+ for(ii = 0; ii < N_CHANNELS; ii++) {
+ if(ii == 0) printf("BEFORE: %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblVelocity_mmps[0],dblMeasuredSampleTime);
//calculate next step in linear path
- mutChannel[ii].lock();//lock relevant mutex
+ //mutPathIn.lock();//lock relevant mutex
dblLinearPathCurrentPos_mm[ii] = dblLinearPathCurrentPos_mm[ii] + dblVelocity_mmps[ii]*dblMeasuredSampleTime; // PATH_SAMPLE_TIME_S SHOULD BE MEASURED
+ if(dblLinearPathCurrentPos_mm[ii] < 0.0) {
+ dblLinearPathCurrentPos_mm[ii] = 0.00;
+ }
+
//check tolerance
- if (fabs(dblLinearPathCurrentPos_mm[ii] - dblTargetActLen_mm[ii]) <= dblPathTolerance)
- {
- dblVelocity_mmps[ii] = 0; //stop linear path generation when linear path is within tolerance of target position.
+ if (fabs(dblLinearPathCurrentPos_mm[ii] - dblTargetActPos_mm[ii]) <= DBL_PATH_TOLERANCE) {
+ dblVelocity_mmps[ii] = 0.0; //stop linear path generation when linear path is within tolerance of target position.
}
- mutChannel[ii].unlock();//unlock relevant mutex
+ //mutPathIn.unlock();//unlock relevant mutex
//calculate next step in smooth path
- dblSmoothPathCurrentPos_mm[ii] = dblSmoothingFactor*dblLinearPathCurrentPos_mm[ii] + (1.0-dblSmoothingFactor)*dblSmoothPathCurrentPos_mm[ii];
-
- //convert to actuator distances
- dblPathToActuator[ii] = dblSmoothPathCurrentPos_mm[ii];
-
- intDemandPos_Tx[ii] = (int) ((dblPathToActuator[ii]/MAX_ACTUATOR_LENGTH)*8191);//convert to a 13-bit number;
-
+ dblSmoothPathCurrentPos_mm[ii] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[ii] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[ii];
+ //mutChannel[ii].lock();
+ intDemandPos_Tx[ii] = (int) ((dblSmoothPathCurrentPos_mm[ii]/MAX_ACTUATOR_LENGTH)*8191);//convert to a 13-bit number;
intDemandPos_Tx[ii] = intDemandPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit
-
- dblPressure_bar[ii] = ReadADCPressure(ii);
- dblPosition_mtrs[ii] = ReadADCPosition(ii);
+ //mutChannel[ii].unlock();
+ if(ii == 0) printf("AFTER: %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[ii],dblVelocity_mmps[ii],dblMeasuredSampleTime);
isDataReady[ii] = 1;//signal that data ready
- }
- printf("%f, %d\r\n",dblPathToActuator[0], intDemandPos_Tx[0]);
- //printf("%f,%f, %f, %f\r\n", dblTargetActLen_mm[0], dblSmoothPathCurrentPos_mm[0],dblPressure_bar[0], dblPosition_mtrs[0]);
- }
-}
-
-void SimulateDemand() // For testing purposes
-{
- int kk = 0;
- while(1)
- {
- mutChannel[0].lock();
- if(kk == 0)
- {
- dblPSI[0][0] = (double) 10.0;
- dblTargetTime_s = 1.5;
- }
- else
- {
- dblPSI[0][0] = (double) 0.0;
- dblTargetTime_s = 2.0;
- }
-
- kk = 1 - kk;
-
-
- //semReplan.release();
-
- mutChannel[0].unlock();
- ReplanPath();
-
- Thread::wait(7000);
- }
+ } // end for
+ //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
+ //printf("%f,%f, %f, %f\r\n", dblTargetActPos_mm[0], dblSmoothPathCurrentPos_mm[0],dblPressure_bar[0], dblPosition_mtrs[0]);
+ } // end while
}
@@ -486,13 +372,7 @@
int main()
{
- for (ii = 0; ii <250; ii++)
- {
- pinCheck = 1;
- wait(0.001);
- pinCheck = 0;
- wait(0.001);
- }
+ int ii;
//initialise relevant variables
for(ii = 0; ii<N_CHANNELS; ii++)
@@ -507,7 +387,7 @@
}
//calculate some control variables
- dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance in mm.
+ //dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance in mm.
pinReset = 1; //initialise reset pin to not reset the controllers.
wait(0.1);
@@ -560,12 +440,13 @@
timer.start();
- threadSimulateDemand.start(SimulateDemand);
- threadPathPlan.start(CalculateSmoothPath); //start planning thread
+ //threadSimulateDemand.start(SimulateDemand);
+ threadReceiveAndReplan.start(ReceiveAndReplan);//start Replanning thread
+ //ReceiveAndReplan();
+ threadSmoothPathPlan.start(CalculateSmoothPath); //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);