Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Diff: main.cpp
- Revision:
- 51:c03f63c6f930
- Parent:
- 50:d1b983a0dd6f
- Child:
- 53:89d16b398615
--- 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); } }