Skad00sh
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Revision 51:c03f63c6f930, committed 2019-03-22
- Comitter:
- adehadd
- Date:
- Fri Mar 22 21:58:48 2019 +0000
- Parent:
- 50:d1b983a0dd6f
- Child:
- 53:89d16b398615
- Commit message:
- ife commit
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Mar 22 19:05:40 2019 +0000
+++ b/main.cpp Fri Mar 22 21:58:48 2019 +0000
@@ -86,6 +86,12 @@
PwmOut pwmCtrl(PWMpin);
+
+//Encoder inputs
+InterruptIn CHA(CHApin);
+InterruptIn CHB(CHBpin);
+
+
//Drive state to output table
const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
@@ -116,7 +122,8 @@
volatile uint32_t _motorTorque; // Motor Toque
volatile uint64_t _newKey; // hash key
-
+ volatile int32_t _motor_pos;
+
char _inCharQ[MAXCMDLENGTH],
_newCmd[MAXCMDLENGTH];
@@ -148,6 +155,9 @@
_targetRot = 459.0;
_targetVel = 45.0;
+ _motor_pos = 0;
+
+
_modeBitField = 0x01; // Default velocity mode
_pc.printf("\n\r%s %d\n\r>", "Welcome", _MAXCMDLENGTH ); // Welcome
@@ -230,12 +240,15 @@
case 'V': // velIn
sscanf(_newCmd, "V%f", &_targetVel); // Find desired the target velocity
_modeBitField = 0x01; // Adjust bitfield pos 1
+ _motor_pos = 0;
putMessage(velIn, _targetVel); // Print it out
break;
case 'R': // posIn
sscanf(_newCmd, "R%f", &_targetRot); // Find desired target rotation
_modeBitField = 0x02; // Adjust bitfield pos 2
+ _targetVel = 2e3;
+ _motor_pos = 0;
putMessage(posIn, _targetRot); // Print it out
break;
@@ -415,7 +428,7 @@
uint8_t _theStates[3], // The Key states
_stateCount[3]; // State Counter
- uint32_t mtrPeriod, // Motor period
+ uint32_t _mtrPeriod, // Motor period
_MAXPWM_PRD;
float _dutyC; // 1 = 100%
bool _RUN;
@@ -423,18 +436,28 @@
Comm* _pComm;
Thread _tMotorCtrl; // Thread for motor Control
+ int8_t old_rotor_state;
+ uint8_t encState;
+ uint32_t quadratureStates;
+ uint32_t MINPWM_PRD;
+ uint32_t maxEncCount;
+ uint32_t encCount;
+ uint32_t encTotal;
+ uint32_t badEdges;
+
public:
//-- Default Constructor With Thread Object Constructor -------------------------------------------------------------//
- Motor() : _tMotorCtrl(osPriorityAboveNormal2, 1024){
+ Motor() : _tMotorCtrl(osPriorityAboveNormal2, 2048){
_dutyC = 1.0f; // Set Power to maximum to drive motorHome()
- mtrPeriod = 2e3; // Motor period
- pwmCtrl.period_us(mtrPeriod); // Initialise PWM
- pwmCtrl.pulsewidth_us(mtrPeriod);
+ _mtrPeriod = 2e3; // Motor period
+ pwmCtrl.period_us(_mtrPeriod); // Initialise PWM
+ pwmCtrl.pulsewidth_us(_mtrPeriod);
_orState = motorHome(); // Rotor offset at motor state 0
_currentState = readRotorState(); // Current Rotor State
_lead = 2; // 2 for forwards, -2 for backwards
+ old_rotor_state = _orState; // Set old_rotor_state to the origin to begin with
_theStates[0] = _orState +1; // Initialise repeatable states, for us this is state 1
_theStates[1] = (_orState + _lead) % 6 +1; // state 3
@@ -449,8 +472,42 @@
_MAXPWM_PRD = 2e3;
+ maxEncCount = 0;
+ encCount = 0;
+ encTotal = 0;
+ badEdges = 0;
+
+ quadratureStates = 1112;
+
}
+ int findMinTorque() {
+ int8_t prevState = readRotorState();
+ uint32_t _mtPeriod = 700;
+ Timer testTimer;
+ testTimer.start();
+
+ _pComm->_pc.printf("PState:%i, CState:%i\n", prevState, readRotorState());
+
+ // stateUpdate();
+
+ while (readRotorState() == prevState) {
+ testTimer.reset();
+ // pwmCtrl.period_us(2e3);
+ pwmCtrl.pulsewidth_us(_mtPeriod);
+ stateUpdate();
+
+ while (testTimer.read_ms() < 500) {}
+ // prevState = readRotorState();
+ _mtPeriod += 10;
+ _mtPeriod = _mtPeriod >= 1000 ? 700 : _mtPeriod;
+ }
+
+ _pComm->_pc.printf("Min Torque:%i\n", _mtPeriod);
+ return _mtPeriod;
+ }
+
+
//-- Start Motor and Attach State Update Function to Rise/Fall Interrupts -------------------------------------------//
void motorStart(Comm *comm) {
@@ -465,11 +522,13 @@
_dutyC = 0.8; // Default a lower duty cycle
- pwmCtrl.period_us((uint32_t)mtrPeriod);
- pwmCtrl.pulsewidth_us((uint32_t)(mtrPeriod*_dutyC));
+ pwmCtrl.period_us((uint32_t)_mtrPeriod);
+ pwmCtrl.pulsewidth_us((uint32_t)(_mtrPeriod*_dutyC));
_pComm = comm;
_RUN = true;
+ MINPWM_PRD = 920;
+
_tMotorCtrl.start(callback(this, &Motor::motorCtrlFn)); // Start motor control thread
@@ -515,15 +574,56 @@
void stateUpdate() { // () { // **params
_currentState = readRotorState(); // Get current state
- if (_currentState == _theStates[0]) // Cumulative state counter (1 for our group's motor)
- _stateCount[0]++;
- else if (_currentState == _theStates[1]) // Cumulative state counter (3 for our group's motor)
- _stateCount[1]++;
- else if (_currentState == _theStates[2]) // Cumulative state counter (5 for our group's motor)
- _stateCount[2]++;
-
motorOut((_currentState - _orState + _lead + 6) % 6); // Send the next state to the motor
+ if (_currentState == 1) {
+ //if (encCount > maxEncCount) maxEncCount = encCount;
+ //if (encCount < minEncCount) minEncCount = encCount;
+ //if (badEdges > maxBadEdges) maxBadEdges = badEdges;
+
+ //revCount++;
+ encTotal += encCount;
+ encCount = 0;
+ badEdges = 0;
+ }
+
+ if (_currentState - old_rotor_state == 5) {
+ _pComm->_motor_pos--;
+ } else if (_currentState - old_rotor_state == -5) {
+ _pComm->_motor_pos++;
+ } else {
+ _pComm->_motor_pos += (_currentState - old_rotor_state);
+ }
+
+ old_rotor_state = _currentState;
+ }
+
+ // A Rise
+ void encISR0() {
+ if (encState == 3) {encCount++;}
+ else badEdges++;
+ encState = 0;
+ }
+
+ // B Rise
+ void encISR1() {
+ if (encState == 0) {encCount++;}
+ else badEdges++;
+ encState = 1;
+ }
+
+ // A Fall
+ void encISR2() {
+ if (encState == 1) {encCount++;}
+ else badEdges++;
+ encState = 2;
+ }
+
+ // B Fall
+ void encISR3() {
+ if (encState == 2) {encCount++;}
+ else badEdges++;
+ encState = 3;
}
//-- Motor PID Control ---------------------------------------------------------------------------------------------//
@@ -542,33 +642,42 @@
int32_t totalDegrees;
int32_t stateDiff;
- int32_t cur_speed; // Variable for local velocity calculation
- int32_t locMotorPos; // Local copy of motor position
+ // int32_t cur_speed; // Variable for local velocity calculation
+ // int32_t locMotorPos; // Local copy of motor position
volatile int32_t torque; // Local variable to set motor torque
static int32_t oldTorque =0;
float sError, // Velocity error between target and reality
- rError; // Rotation error between target and reality
+ rError,
+ eError = 0; // Rotation error between target and reality
static float rErrorOld; // Old rotation error used for calculation
+ float encRemain = 0;
+ bool attached = true;
+ bool declared = false;
//~~~Controller constants~~~~
- int32_t Kp1=22; // Proportional controller constants
- int32_t Kp2=22; // Calculated by trial and error to give optimal accuracy
- int32_t Ki = 12;
- float Kd=15.5;
-
+ int32_t Kp1=90; // Proportional controller constants
+ int32_t Kp2=33; // Calculated by trial and error to give optimal accuracy
+ float Kd=36.5;
+ float Kis = 10.0f; // 50
+ int32_t Kir = 0.0f;
+ float sIntegral = 0.0f;
+ float rIntegral = 0.0f;
int32_t Ts; // Initialise controller output Ts (s=speed)
int32_t Tr; // Initialise controller output Tr (r=rotations)
- int32_t old_pos = 0;
+ int32_t old_pos = 0,
+ cur_pos = 0;
- uint32_t cur_time = 0,
- old_time = 0,
- time_diff;
+ float cur_err = 0.0f,
+ old_err = 0.0f,
+ err_diff,
+ time_diff,
+ hold_pos = 0,
+ cur_time = 0.0f,
+ old_time = 0.0f,
+ cur_speed = 0.0f;
- float cur_err = 0.0f,
- old_err = 0.0f,
- err_diff;
enum { // To nearest Hz
@@ -589,52 +698,200 @@
core_util_critical_section_enter(); // Access shared variables here
cpyModeBitfield = _pComm->_modeBitField;
- std::copy(_stateCount, _stateCount+3, cpyStateCount);
- cpyCurrentState = _currentState;
- for (int i = 0; i < 3; ++i) {
- _stateCount[i] = 0;
- }
+ // std::copy(_stateCount, _stateCount+3, cpyStateCount);
+ // cpyCurrentState = _currentState;
+ // for (int i = 0; i < 3; ++i) {
+ // _stateCount[i] = 0;
+ // }
core_util_critical_section_exit();
- cur_time = m_timer.read(); // Read state & timestamp
+ // cur_time = m_timer.read(); // Read state & timestamp
- time_diff = cur_time - old_time;
+ // time_diff = cur_time - old_time;
// cur_speed = (cur_pos - old_pos) / time_diff;
- old_time = cur_time; // prep values for next time through loop
- old_pos = cpyCurrentState;
+ // old_time = cur_time; // prep values for next time through loop
+ // old_pos = cpyCurrentState;
// Hence we make the value positive,// and instead set the direction to the opposite one
- iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount;
+ // iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount;
- totalDegrees = ting[0] * cpyStateCount[iterElementMax];
- stateDiff = _theStates[iterElementMax]-cpyCurrentState;
- stateDiff >= 0 ? totalDegrees = totalDegrees + (ting[1]* stateDiff) : \
- totalDegrees = totalDegrees + (ting[1]* stateDiff *-1);
+ // totalDegrees = ting[0] * cpyStateCount[iterElementMax];
+ // stateDiff = _theStates[iterElementMax]-cpyCurrentState;
+ // stateDiff >= 0 ? totalDegrees = totalDegrees + (ting[1]* stateDiff) : \
+ // totalDegrees = totalDegrees + (ting[1]* stateDiff *-1);
if ((cpyModeBitfield & 0x01)|(cpyModeBitfield & 0x02)) {// Speed, torque control and PID
- cur_speed = totalDegrees / time_diff;
- sError = (_pComm->_targetVel * 6) - abs(cur_speed); // Read global variable _targetVel updated by interrupt and calculate error between target and reality
+ if (abs(cur_speed) <= 120 && (!attached)) {
+ eError = 1.0f;
+ attached = true;
+
+ CHA.rise(callback(this, &Motor::encISR0));
+ CHB.fall(callback(this, &Motor::encISR3));
+ CHB.rise(callback(this, &Motor::encISR1));
+ CHA.fall(callback(this, &Motor::encISR2));
+
+ encCount = 0;
+ encTotal = 0;
+ //_comm->printf("old error:%.4f quadratureStates:%u", old_err, quadratureStates);
+ encRemain = old_err * quadratureStates; // Number of encs remaining
+ //_pComm->_pc.printf("old error:%.4f quadratureStates:%u encRemain:%.4f", old_err, quadratureStates, encRemain);
+ }
+
+ else if (abs(cur_speed) > 120 && (attached)) {
+ eError = 0.0f;
+ attached = false;
+
+ CHA.rise(NULL);
+ CHB.fall(NULL);
+ CHB.rise(NULL);
+ CHA.fall(NULL);
+
+ encCount = 0;
+ encTotal = 0;
+ eError = 0;
+
+ // Store the number of rotations that have been done up to the point where the channels are activated
+ hold_pos = (cur_pos / 6.0f);
+ _pComm->_pc.printf("HPos:%f\n", hold_pos);
+
+ }
- // Ts = Kp * (s -|v|) where, // SPEED CONTROLLER
- // Ts = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity
+ // read state & timestamp
+ cur_pos = _pComm->_motor_pos;
+ if (cur_pos < 2) {
+ rIntegral = 0.0f;
+ sIntegral = 0.0f;
+ declared = false;
+ }
+ cur_time = m_timer.read();
+
+ // compute speed
+ time_diff = cur_time - old_time;
+ cur_speed = (cur_pos - old_pos) / time_diff;
+
+ // prep values for next time through loop
+ old_time = cur_time;
+ old_pos = cur_pos;
+
+ // Position error
+ // if (!attached) {
+ rError = (_pComm->_targetRot) - (cur_pos/6.0f);
+ // }
+ // else{
+ // rErrorOld = RError;
+ // rError = (_pComm->_targetRot) - (hold_pos + ((encRemain - encTotal)/quadratureStates));
+ // }
+
+
+ if ((cur_speed != 0)) {
+ rIntegral += rError * time_diff;
+ }
+ err_diff = rError - old_err;
+ old_err = rError;
+
+ // Speed error - Convert curr_speed from states per time to rotations per time
+ // TODO: Check the direction that CPos goes in when _targetVel < 0
+ sError = (_pComm->_targetVel) - (abs(cur_speed/6.0f)); //Read global variable _targetVel updated by interrupt and calculate error between target and reality
+ if ((cur_speed != 0) && (torque != _MAXPWM_PRD)) {
+ sIntegral += sError * time_diff;
+ if (abs(sIntegral * Kis) >= 2000) {
+ sIntegral -= sError * time_diff;
+ }
+ }
+
+ Ts = (int32_t)(((Kp1 * sError) + (Kis * sIntegral))); // * sgn(cur_pos));
+ Tr = (int32_t)(Kp2*rError + ((Kd/time_diff) * err_diff) + (Kir * rIntegral));
- // Check if user entered V0 and set the output to maximum as specified
- sError == -abs(cur_speed) ? Ts = _MAXPWM_PRD : \
- Ts = (Kp1 * sError); // If the user didn't enter V0 implement controller transfer function:
-
-
- // Tr= Kp*Er + Kd* (dEr/dt) where, // ROTATION CONTROLLER
- // Tr = controller output, Kp = prop controller constant, Er = error in number of rotations
- rError = (_pComm->_targetRot)*6 - totalDegrees; // Read global variable _targetRot updated by interrupt and calculate the rotation error.
- Tr = Kp2*rError + Kd*(rError - rErrorOld); // Implement controller transfer function
- rErrorOld = rError; // Update rotation error
-
- Ts = (int32_t)( Ts * sgn(rError) ); // Use the sign of the error to set controller wrt direction of rotation
+ if (cpyModeBitfield & 0x01) {
+ // Speed control
+ if (_pComm->_targetVel == 0) { //Check if user entered V0,
+ torque = _MAXPWM_PRD; //and set the output to maximum as specified
+ }
+
+ else {
+ // select minimum absolute value torque
+ /*
+ if (cur_speed < 0) {
+ torque = max(Ts, Tr);
+ }
+
+ else {
+ torque = min(Ts, Tr);
+ } */
+
+ torque = Ts;
+ if (abs(sError) > 0.6) {
+ // torque = abs(Tr) < 1000 ? 1000*sgn(Tr) : Tr;
+ torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque;
+ _pComm->_pc.printf("Speed:%f\r\n", (cur_speed/6.0f));
+ } else {
+ torque = 0;
+ }
+
+ torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque;
+ // torque = Ts;
+ }
+ }
+
+ else if (cpyModeBitfield & 0x02) {
+ // select minimum absolute value torque
+ if (_pComm->_targetRot == 0) {
+ // Not spinning at max speed
+ torque = 0.7 * _MAXPWM_PRD;
+ }
- cur_speed < 0 ? torque = max(Ts, Tr): torque = min(Ts, Tr);
+ else {
+ /*
+ // TODO: Handle sign or Tr for negative rotations
+ if (Tr > MINPWM_PRD || Tr < 100) {
+ torque = Tr;
+ } else if (Tr >= 100 && Tr <= MINPWM_PRD) {
+ torque = MINPWM_PRD;
+ } */
+
+ // select minimum absolute value torque
+ if (cur_speed < 0) {
+ torque = max(Ts, Tr);
+ } else {
+ torque = min(Ts, Tr);
+ }
+
+ if (abs(rError) > 0.2 && (rError > 0)) {
+ // torque = abs(Tr) < 1000 ? 1000*sgn(Tr) : Tr;
+ torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque;
+ } else {
+ torque = 0;
+ if (!declared) {
+ _pComm->_pc.printf("Remaining Turns:%f\n", rError);
+ declared = true;
+ }
+ //attached = false;
+ }
+
+ }
+ }
+
+ if (torque < 0) {
+ torque = -torque;
+ _lead = -2;
+ } else {
+ _lead = 2;
+ }
+
+ torque = torque > _MAXPWM_PRD ? _MAXPWM_PRD : torque; // Set a cap on torque
+
+ _pComm->_motorTorque = torque;
+ pwmCtrl.pulsewidth_us(torque);
+ // _pComm->_pc.printf("EError:%.4f, tot:%d, encRemain:%.4f || RError:%.4f || SError:%.4f, CSpeed:%f, Torque:%i\r\n", eError, encTotal, encRemain, rError, sError, (cur_speed/6.0f), torque);
+
+ //_pComm->_pc.printf("EError:%.4f, remain:%.4f, tot%d || RError:%.4f || Torque:%i \r\n", eError, encRemain, encTotal, rError, torque);
+
+
+ // Give the motor a kick
+ stateUpdate();
}
else if (cpyModeBitfield & 0x04) { // If it is in torque mode, do no PID math, just set pulsewidth
@@ -645,10 +902,10 @@
}
}
else if (cpyModeBitfield & 0x08) { // If it is in Melody mode
- uint32_t oldMtrPeriod = mtrPeriod; // Backup of current motor period
+ uint32_t oldMtrPeriod = _mtrPeriod; // Backup of current motor period
float oldDutyC = _dutyC; // Backup of current duty cycle
uint32_t noteFreq, newDur, newPeriod =0,
- oldPeriod = mtrPeriod;
+ oldPeriod = _mtrPeriod;
Timer testTimer;
testTimer.start();
@@ -686,6 +943,8 @@
break;
}
+ if (noteFreq == 0) {break;} // leave the loop if we get a wrong character
+
newPeriod = (uint32_t)(1e6/(uint32_t)noteFreq); // us
if (newPeriod>oldPeriod) { // Change Period First
@@ -717,13 +976,13 @@
}
- torque < 0 ? _lead = -2 : _lead = +2;
- torque = abs(torque);
+ // torque < 0 ? _lead = -2 : _lead = +2;
+ // torque = abs(torque);
- if(torque > _MAXPWM_PRD) torque = _MAXPWM_PRD; // In case the calculated PWM is higher than our maximum 50% allowance,
- // Set it to our max.
- _pComm->_motorTorque = torque;
- pwmCtrl.pulsewidth_us(torque);
+ // if(torque > _MAXPWM_PRD) torque = _MAXPWM_PRD; // In case the calculated PWM is higher than our maximum 50% allowance,
+ // // Set it to our max.
+ // _pComm->_motorTorque = torque;
+ // pwmCtrl.pulsewidth_us(torque);
}
}