Skad00sh
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Diff: main.cpp
- Revision:
- 39:05a021718517
- Parent:
- 38:a3713a09c828
- Child:
- 40:9fae84f111e6
--- a/main.cpp Wed Mar 20 08:30:45 2019 +0000
+++ b/main.cpp Wed Mar 20 10:01:38 2019 +0000
@@ -92,7 +92,7 @@
volatile uint8_t cmdIndx;
volatile uint8_t inCharQIdx;
- volatile uint16_t motorPower; // motor toque
+ volatile uint32_t motorPower; // motor toque
volatile float targetVel;
volatile float targetRot;
@@ -112,6 +112,8 @@
} msg;
Mail<msg, 32> mailStack;
+
+ int8_t modeBitfield; // 0,0,0,0,0,Torque,Rotation,Velocity
void serialISR(){
if (pc.readable()) {
@@ -170,14 +172,17 @@
break;
case 'V': //(MsgChar[velIn])://
sscanf(newCmd, "V%f", &targetVel); //Find desired the target velocity
+ modeBitfield = 0x01;
putMessage(velIn, targetVel); //Print it out
break;
case 'R': //(MsgChar[posIn])://
sscanf(newCmd, "R%f", &targetRot); //Find desired target rotation
+ modeBitfield = 0x02;
putMessage(posIn, targetRot); //Print it out
break;
case 'T': //(MsgChar[torque])://
sscanf(newCmd, "T%u", &motorPower); //Find desired target torque
+ modeBitfield = 0x04;
putMessage(torque, motorPower); //Print it out
break;
case 'M': //(MsgChar[torque])://
@@ -220,22 +225,22 @@
pc.printf("\r>%s< New Key Added:\t0x%016x\n\r", inCharQ, pMessage->message);
break;
case torque:
- pc.printf("\r>%s< Motor Torque set to:\t%d\n\r", inCharQ, pMessage->message);
+ pc.printf("\r>%s< Motor Torque set to:\t%d\n\r", inCharQ, (int32_t) pMessage->message);
break;
case velIn:
pc.printf("\r>%s< Target Velocity set to:\t%.2f\n\r", inCharQ, targetVel);
break;
case velOut:
pc.printf("\r>%s< Current Velocity:\t%.2f\n\r", inCharQ, \
- (float) ((int32_t) pMessage->message / 6));
+ (float) ((int32_t) pMessage->message /*/ 6*/));
break;
case posIn:
pc.printf("\r>%s< Target Rotation set to:\t%.2f\n\r", inCharQ, \
- (float) ((int32_t) pMessage->message / 6));
+ (float) ((int32_t) pMessage->message /*/ 6*/));
break;
case posOut:
pc.printf("\r>%s< Current Position:\t%.2f\n\r", inCharQ, \
- (float) ((int32_t) pMessage->message / 6));
+ (float) ((int32_t) pMessage->message /*/ 6*/));
break;
case error:
pc.printf("\r>%s< Debugging position:%x\n\r", inCharQ, pMessage->message);
@@ -297,7 +302,7 @@
targetVel = 45.0;
targetRot = 459.0;
-
+ modeBitfield = 0x01; // Default is velocity mode
/*MsgChar = {'m', 'R', 'V', 'r', 'v',
@@ -359,7 +364,7 @@
volatile int8_t stateList[6]; //All possible rotor states stored
//Phase lead to make motor spin
- int8_t lead;
+ volatile int8_t lead;
Comm* p_comm;
bool _RUN;
@@ -367,13 +372,13 @@
//Run the motor synchronisation
float dutyC; // 1 = 100%
- uint16_t mtrPeriod; // motor period
+ uint32_t mtrPeriod; // motor period
uint8_t stateCount[3]; // State Counter
uint8_t theStates[3]; // The Key states
Thread t_motor_ctrl; // Thread for motor Control
- uint16_t MAXPWM_PRD;
+ uint32_t MAXPWM_PRD;
public:
@@ -420,8 +425,8 @@
// Default a lower duty cylce
dutyC = 0.8;
- pwmCtrl.period_us((uint16_t)mtrPeriod);
- pwmCtrl.pulsewidth_us((uint16_t)mtrPeriod*dutyC);
+ pwmCtrl.period_us((uint32_t)mtrPeriod);
+ pwmCtrl.pulsewidth_us((uint32_t)mtrPeriod*dutyC);
p_comm = comm;
_RUN = true;
@@ -499,17 +504,18 @@
// Init some things
uint8_t cpyStateCount[3];
uint8_t cpyCurrentState;
+ int8_t cpyModeBitfield;
- int16_t ting[2] = {5,1}; // 360,60 (for degrees), 5,1 (for states)
+ int32_t ting[2] = {5,1}; // 360,60 (for degrees), 5,1 (for states)
uint8_t iterElementMax;
- int16_t totalDegrees;
- int16_t stateDiff;
+ int32_t totalDegrees;
+ int32_t stateDiff;
int32_t velocity; //Variable for local velocity calculation
int32_t locMotorPos; //Local copy of motor position
// static int32_t oldMotorPos = 0; //Old motor position used for calculations
// static uint8_t motorCtrlCounter = 0; //Counter to be reset every 10 iterations to get velocity calculation in seconds
- int16_t torque; //Local variable to set motor torque
+ volatile int32_t torque; //Local variable to set motor torque
float sError; //Velocity error between target and reality
float rError; //Rotation error between target and reality
static float rErrorOld; //Old rotation error used for calculation
@@ -519,10 +525,16 @@
int32_t Kp2=22; //Calculated by trial and error to give optimal accuracy
float Kd=15.5;
+
+ int32_t Ys; //Initialise controller output Ys (s=speed)
+ int32_t Yr; //Initialise controller output Yr (r=rotations)
while (_RUN) {
t_motor_ctrl.signal_wait((int32_t)0x1);
core_util_critical_section_enter();
+
+ cpyModeBitfield = p_comm->modeBitfield;
+ p_comm->modeBitfield = 0;
//Access shared variables here
std::copy(stateCount, stateCount+3, cpyStateCount);
// TODO: A thing yes
@@ -544,41 +556,60 @@
}
//p_comm->pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10));
- //~~~~~Speed controller~~~~~~
- velocity = totalDegrees*10;
- sError = (p_comm->targetVel * 6) - abs(velocity); //Read global variable targetVel updated by interrupt and calculate error between target and reality
- int32_t Ys; //Initialise controller output Ys (s=speed)
- if (sError == -abs(velocity)) { //Check if user entered V0,
- Ys = MAXPWM_PRD; //and set the output to maximum as specified
- } else {
- Ys = (int32_t)(Kp1 * sError); //If the user didn't enter V0 implement controller transfer function: Ys = Kp * (s -|v|) where,
- } //Ys = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity
+ if ((cpyModeBitfield & 0x01) | (cpyModeBitfield & 0x02)) {
+ //~~~~~Speed controller~~~~~~
+ velocity = totalDegrees*10;
+ sError = (p_comm->targetVel * 6) - abs(velocity); //Read global variable targetVel updated by interrupt and calculate error between target and reality
+
+ if (sError == -abs(velocity)) { //Check if user entered V0,
+ Ys = MAXPWM_PRD; //and set the output to maximum as specified
+ } else {
+ Ys = (int32_t)(Kp1 * sError); //If the user didn't enter V0 implement controller transfer function: Ys = Kp * (s -|v|) where,
+ } //Ys = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity
- //~~~~~Rotation control~~~~~~
- rError = p_comm->targetRot - cpyCurrentState; //Read global variable targetRot updated by interrupt and calculate the rotation error.
- int32_t Yr; //Initialise controller output Yr (r=rotations)
- Yr = Kp2*rError + Kd*(rError - rErrorOld); //Implement controller transfer function Ys= Kp*Er + Kd* (dEr/dt)
- rErrorOld = rError; //Update rotation error
- if(rError < 0){ //Use the sign of the error to set controller wrt direction of rotation
- Ys = -Ys;
+// } else if (cpyModeBitfield & 0x02) {
+ //~~~~~Rotation control~~~~~~
+ rError = p_comm->targetRot - cpyCurrentState; //Read global variable targetRot updated by interrupt and calculate the rotation error.
+ Yr = Kp2*rError + Kd*(rError - rErrorOld); //Implement controller transfer function Ys= Kp*Er + Kd* (dEr/dt)
+ rErrorOld = rError; //Update rotation error
+ if(rError < 0){ //Use the sign of the error to set controller wrt direction of rotation
+ Ys = -Ys;
+ }
}
- if((velocity>=0 && Ys<Yr) || (velocity<0 && Ys>Yr)){ //Choose Ys or Yr based on distance from target value so that it takes
- torque = Ys; //appropriate steps in the right direction to reach target value
- } else {
- torque = Yr;
+ if (cpyModeBitfield & 0x04) { // if it is in torque mode, do no math, just set pulsewidth
+ torque = (int32_t)p_comm->motorPower;
+ if(torque < 0){ //Variable torque cannot be negative since it sets the PWM
+ torque = -torque; //Hence we make the value positive,
+ lead = -2; //and instead set the direction to the opposite one
+ } else {
+ lead = 2;
+ }
+ if(torque > MAXPWM_PRD){ //In case the calculated PWM is higher than our maximum 50% allowance,
+ torque = MAXPWM_PRD; //Set it to our max.
+ p_comm->putMessage((Comm::msgType)8, torque);
+ }
+ p_comm->motorPower = torque;
+ pwmCtrl.pulsewidth_us(torque);
+ } else { // if not Torque mode
+ if((velocity>=0 && Ys<Yr) || (velocity<0 && Ys>Yr)){ //Choose Ys or Yr based on distance from target value so that it takes
+ torque = Ys; //appropriate steps in the right direction to reach target value
+ } else {
+ torque = Yr;
+ }
+ if(torque < 0){ //Variable torque cannot be negative since it sets the PWM
+ torque = -torque; //Hence we make the value positive,
+ lead = -2; //and instead set the direction to the opposite one
+ } else {
+ lead = 2;
+ }
+ if(torque > MAXPWM_PRD){ //In case the calculated PWM is higher than our maximum 50% allowance,
+ torque = MAXPWM_PRD; //Set it to our max.
+ }
+
+ p_comm->motorPower = torque;
+ pwmCtrl.pulsewidth_us(p_comm->motorPower);
}
- if(torque < 0){ //Variable torque cannot be negative since it sets the PWM
- torque = -torque; //Hence we make the value positive,
- lead = -2; //and instead set the direction to the opposite one
- } else {
- lead = 2;
- }
- if(torque > MAXPWM_PRD){ //In case the calculated PWM is higher than our maximum 50% allowance,
- torque = MAXPWM_PRD; //Set it to our max.
- }
-
- pwmCtrl.pulsewidth_us(p_comm->motorPower);
// pwmCtrl.write((float)(p_comm->motorPower/MAXPWM_PRD));
// p_comm->motorPower = torque; //Lastly, update global variable motorPower which is updated by interrupt
// p_comm->pc.printf("\t\t\t\t\t\t %i, %i, %i \r", torque, Ys, Yr);