MorphGI
Low level control for MorphGI Control unit rebuild, using PWM to receive commands from Mid level.
Dependencies: mbed QEI FastAnalogIn mbed-rtos FastPWM
Diff: main.cpp
- Revision:
- 2:aee7d4724915
- Parent:
- 1:cb2859df7a4c
- Child:
- 3:9bd35e5b05ba
--- a/main.cpp Mon Dec 17 15:54:28 2018 +0000
+++ b/main.cpp Mon Jan 28 15:30:36 2019 +0000
@@ -20,12 +20,13 @@
const float MAX_SPEED_MMPS = 24.3457;
-
+int intDummy;
SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel
QEI wheel (PB_5, PB_4, NC, 256, QEI::X4_ENCODING);
DigitalOut Mntr(D3);
+DigitalOut Mntr2(PB_8);
#define EXTERNAL_CLOCK_MODE 0x08
#define RANGE_CONFIG 0x03
@@ -126,7 +127,7 @@
int dataReceived;
//sample time variables
- double dblSampleTime_s = 0.001;
+ double dblSampleTime_s = 0.99;
//system state variables
@@ -151,6 +152,9 @@
double dblTargetPos;
double dblTargetVel;
+ double dblLinearPath;
+ double dblLastLinearPath;
+ double dblPathSign;
double dblMaxPos = 41.0; //maximum actuator position position in mm
@@ -166,6 +170,8 @@
int intDemVelFilOrder = 6;
int intOutFilOrder = 0;
+ double dblPressureVal_bar;
+
//controller variables
double omega;
double zeta;
@@ -175,7 +181,7 @@
double Kd = 1.5;
double dblIntTerm;
double dblIntLimit = 0.8;
-
+ int RXFlag;
double dblControlBias = 0.0;
double dblMotorVoltage = 12.0;
@@ -323,8 +329,9 @@
while(1)
{
semPosCtrl.wait();
-
- Mntr = 1;//!led;
+ Mntr = !Mntr;
+ //Mntr2 = 1;
+ //Mntr2 = 1 - Mntr2;//!led;
pulsesTest = wheel.getPulses();
double testy = (double) abs(pulsesTest)/2000;
@@ -335,12 +342,25 @@
//sensor readings
intPressureRead = (Read14BitADC(PRESSURE_CHAN, cs_ADC));//read pressure
- intPressureRead = intPressureRead-1333;
- intPressureRead = intPressureRead/11997 * 511;
- intFeedBack_pres = (intFeedBack_pres<<5) | SumDigits(intFeedBack_pres);
- intFeedBack_pres = (intFeedBack_pres <<1) &0x1;
- intFeedBack_pres = (intFeedBack_pres <<1) | EvenParityBitGen(intFeedBack_pres);
-
+ intPressureRead = intPressureRead-1334;
+
+ dblPressureVal_bar = ( (double) intPressureRead/10667)*10.0;
+
+
+ intFeedBack_pres = (int)(((double)intPressureRead/10667) * 511);
+ //printf("%d\r\n",intFeedBack_pres);
+
+
+ //intFeedBack_pres = intFeedBack_pres>>5;
+
+ intFeedBack_pres = (intFeedBack_pres<<5) | SumDigits(intFeedBack_pres);//add checksum
+ intFeedBack_pres = (intFeedBack_pres<<1);
+ intFeedBack_pres = intFeedBack_pres | 0x0001; //add type (1 for pressure)
+ intFeedBack_pres = (intFeedBack_pres <<1) | EvenParityBitGen(intFeedBack_pres);//add parity
+ intFeedBack_pres = intFeedBack_pres & 0xFFFF;
+
+ unsigned short garb = 0x01;
+
intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
dblPotPositionRead = (double) POT_2_MM*(intPotRead - POT_OFFSET);
@@ -392,7 +412,7 @@
//printf("%f\r\n",dblPosD[intDemPosFilOrder]);
- intFeedBack_pos = dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH*511;
+ intFeedBack_pos = (int) dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH*511;
if(intFeedBack_pos>511)
{
@@ -405,28 +425,30 @@
}
- intFeedBack_pos = (intFeedBack_pos<<5) | SumDigits(intFeedBack_pos);
- intFeedBack_pos = intFeedBack_pos <<1;
- intFeedBack_pos = (intFeedBack_pos <<1) | EvenParityBitGen(intFeedBack_pos);
+ intFeedBack_pos = (intFeedBack_pos<<5) | SumDigits(intFeedBack_pos);//add checkSum
+ intFeedBack_pos = intFeedBack_pos <<1; // add type (0 for position)
+ intFeedBack_pos = (intFeedBack_pos <<1) | EvenParityBitGen(intFeedBack_pos);//add parity
//intFeedBack = dblSensorDriftError*8191;
-
-
-
-
+
///////////////PATH GENERATION////////////////////////
//work out next path point
double dblPathDifference;
dblPathDifference = dblTargetPos - dblPosD[intDemPosFilOrder];
- double dblLinearPath;
- double dblLastLinearPath;
+
//check if target has been reached
- if (dblPathDifference > 1.05*MAX_SPEED_MMPS*dblSampleTime_s) {
+ if (fabs(dblPathDifference) > 1.05*MAX_SPEED_MMPS*dblSampleTime_s) {
//is velocity positive or negative?
- double dblPathSign = dblPathDifference/fabs(dblPathDifference);
+ dblPathSign = dblPathDifference/fabs(dblPathDifference);
dblLinearPath = dblLinearPath + dblPathSign*dblTargetVel*dblSampleTime_s;//next point in path
+ if(dblLinearPath > dblMaxPos){
+ dblLinearPath = dblMaxPos;
+ }
+ if (dblLinearPath < 0.0){
+ dblLinearPath = 0.0;
+ }
//now smooth
dblPosD[intDemPosFilOrder] = 0.2*dblLinearPath + 0.8*dblPosD[intDemPosFilOrder];
@@ -447,6 +469,10 @@
dblVelD[0] = dblPosD[intDemPosFilOrder] - dblLastPosD;
+
+ ///////////////////////////////////////////////////// End of Path Generation
+
+
//run PID calculations
//get errors
dblError = dblPosD[intDemPosFilOrder] - dblPos[intPosFilOrder];
@@ -537,37 +563,79 @@
dblLastPos = dblPos[intPosFilOrder];
dblLastPosD = dblPosD[intDemPosFilOrder];
- Mntr = 0;
-
//GateControl();
+ pinGate = 1;
+
gateTimer.reset();
- pinGate = 1;
+
- while((gateTimer.read_us() < 600) && pinGate == 1)
+ while((gateTimer.read_us() < 800) && pinGate == 1)
{
if(slave.receive())
{
+ //Mntr = !1;
//receive first msg
- slaveReceivePos = slave.read(); //reply with 0xFFFF
+ slaveReceivePos = slave.read(); //reply with 0xFFFF 1
slave.reply(intFeedBack_pos);//prepare position reply
- slaveReceiveVel = slave.read();//get next message
-
- //intFeedBack = intPressureRead & 0xFFFF;
+
+ slaveReceiveVel = slave.read();//get next message, send position reply 2
slave.reply(intFeedBack_pres); //prepare pressure reply
+ //slave.reply(0x5555);
- slave.read();//get next message, send reply
+ intDummy = slave.read();//get next message, send pressure reply 3
+ slave.reply(0xFFFF); //prepare next dummy reply for following msg
+
+ //slave.read();
- slave.reply(0xFFFF); //prepare next dummy reply
+ // intDummy = slave.read();//get next message, send status reply
+//
+
+
pinGate = 0;
-
- //deal with position
+ printf("P:%d\t V:%d\t D:%d\r\n", slaveReceivePos,slaveReceiveVel,intDummy);
+ //deal with pos msg
//find parity & checkSum
intPar_pos = EvenParityBitGen(slaveReceivePos>>1);
intChkSum_pos = SumDigits(slaveReceivePos>>7);
+ bool isParPos = (intPar_pos == (slaveReceivePos&0x1));
+ bool isChkPos = (intChkSum_pos == ((slaveReceivePos>>2)&0x1F));
+ bool isTypePos = (( (slaveReceivePos>>1) &0x1 ) == 0);
+
+
+
+ //deal with velocity msg
+ //find parity & checkSum
+ intPar_vel = EvenParityBitGen(slaveReceiveVel>>1);
+ intChkSum_vel = SumDigits(slaveReceiveVel>>7);
+
+ bool isParVel = (intPar_vel == (slaveReceiveVel&0x1));
+ bool isChkVel = (intChkSum_vel == ((slaveReceiveVel>>2)&0x1F));
+ bool isTypeVel = ( (slaveReceiveVel>>1) &0x1 ) == 1;
+
+ //if(isParVel){
+// Mntr = 1;
+// } else{
+// Mntr = 0;
+// }
+//
+// if(isChkVel){
+// Mntr2 = 1;
+// }
+// else {
+// Mntr2 = 0;
+// }
+
+ //if(isParPos && isChkPos && isTypePos && isParVel && isChkVel && isTypeVel){
+// slave.reply(0x5555);//prepare specific reply that all is well
+// }
+// else {
+// slave.reply(0x0000); // otherwise give it something wrong
+// }
+
//check if parity, check Sum and mesage type matches
- if((intPar_pos == (slaveReceivePos&0x1))&&(intChkSum_pos == ((slaveReceivePos>>2)&0x1F))&&( ( (slaveReceivePos>>1) &0x1) ) == 0) {
+ if(isParPos && isChkPos && isTypePos) {
slaveReceivePos = slaveReceivePos>>7;
dblTargetPos = (double) MAX_ACTUATOR_LENGTH*slaveReceivePos/511;
@@ -581,17 +649,14 @@
dblTargetPos = 0.0;
}
}
-
- //deal with velocity
- //find parity & checkSum
- intPar_vel = EvenParityBitGen(slaveReceiveVel>>1);
- intChkSum_vel = SumDigits(slaveReceiveVel>>7);
-
+ //printf("P:%d\tC:%d\tT:%d\t%f\r\n", slaveReceiveVel&0x1, ((slaveReceiveVel>>2)&0x1F), ((slaveReceiveVel>>1) & 0x1),dblTargetVel);
+
+ //parityFail = 0;
//check if parity, check Sum and mesage type matches
- if((intPar_vel == (slaveReceiveVel&0x1))&&(intChkSum_vel == ((slaveReceiveVel>>2)&0x1F))&&( ( (slaveReceiveVel>>1) &0x1) ) == 0) {
+ if(isParVel && isChkVel && isTypeVel) {
slaveReceiveVel = slaveReceiveVel>>7;
- dblTargetVel = (double) MAX_SPEED_MMPS*slaveReceiveVel/511;
-
+ dblTargetVel = (double) slaveReceiveVel/511.0;
+ dblTargetVel = MAX_SPEED_MMPS*dblTargetVel;
//limit demand to ensure safety
if(dblTargetVel > MAX_SPEED_MMPS)
{
@@ -601,34 +666,16 @@
{
dblTargetVel = 0.0;
}
+
}
-
-
-
-
-// dataFlag = slaveReceive>>13;
-//
-// if(dataFlag == 0)
-// {
-// slaveReceive = slaveReceive>>7 & 0x1FF;
-// //get demand and filter
-// dblPosD[intDemPosFilOrder] = (double) dblMaxPos*slaveReceive/511;
-//
-// //limit demand to ensure safety
-// if(dblPosD[intDemPosFilOrder] > dblMaxPos)
-// {
-// dblPosD[intDemPosFilOrder] = dblMaxPos;
-// }
-// if(dblPosD[intDemPosFilOrder] < 0.0)
-// {
-// dblPosD[intDemPosFilOrder] = 0.0;
-// }
-// }
-
+
+ //Mntr2 = 0;
}
}
pinGate = 0;
-
+
+
+ //printf("Demand Pos: %f\t RXFlag: %d\t parity?%d \r\n",dblTargetPos, RXFlag, parityFail);
}
}
@@ -657,8 +704,11 @@
Ticker positionCtrlTicker;
int main() {
+ pinGate = 0;
+
cs_ADC = 1;
-
+ Mntr = 0;
+ Mntr2 = 0;
pinPwmOutput.period_us(50);
printf("\r\nYo Yo Yo, Low Level innit\r\n\n\n");
wait(0.5);
@@ -676,6 +726,7 @@
//
slave.format(16,2);
slave.frequency(10000000);
+
dblPosD[intDemPosFilOrder] = 0.0;
slaveReceivePos = 0.0;
slaveReceiveVel = 0.0;
@@ -687,13 +738,16 @@
intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
//intPotRead = (Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
dblStartingPos = (double) POT_2_MM*(intPotRead - POT_OFFSET);
+ dblLinearPath = dblStartingPos;
dblTargetPos = dblStartingPos;
+ dblTargetVel = 0.0;
dblPosD[intDemPosFilOrder] = dblStartingPos;
- printf("\r\n%d, %f\r\n", intPotRead, dblStartingPos);
+ printf("\r\nPotRead: %d, Current Pos: %f, Target Pos: %f\r\n", intPotRead, dblStartingPos, dblTargetPos);
//wait(0.05);
+
//printf("\n\n\n");
//dblStartingPos = (double) POT_2_MM*(uintPotRead - POT_OFFSET);
@@ -702,6 +756,7 @@
//calculate/convert variables
CurrentLimitSet = dblCurrentLimitAmps *0.14/3.3;
+ slave.reply(0x5555);
PositionControlThread.start(PositionControlPID);
DutyCycleThread.start(CalculateDutyCycle);
positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s);