Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Diff: main.cpp
- Revision:
- 53:89d16b398615
- Parent:
- 51:c03f63c6f930
- Child:
- 54:bd5b586aa2e1
diff -r c03f63c6f930 -r 89d16b398615 main.cpp --- a/main.cpp Fri Mar 22 21:58:48 2019 +0000 +++ b/main.cpp Fri Mar 22 23:42:55 2019 +0000 @@ -1,23 +1,3 @@ -/*TODO: -Change: - Indx - _newCmd - _MAXCMDLENGTH -move the global variables to a class because we arent paeasents - Mission Failed -use jack's motor motor position -fix class variable naming -dont make everything public becuase thats fucling dumb and defeats the whole point of a class - -Move things out of public and into a protected part of the class - -Move char Comm::_inCharQ[] = {'.','.','... into the class by making it a vector - -Change abs lacro to -NOT V0 but R0 (to go on forever) - -Actually make the code robust lol -*/ - //Mapping from sequential drive states to motor phase outputs /* State L1 L2 L3 @@ -241,6 +221,11 @@ sscanf(_newCmd, "V%f", &_targetVel); // Find desired the target velocity _modeBitField = 0x01; // Adjust bitfield pos 1 _motor_pos = 0; + if (&_targetVel < 0) { + _targetRot = -100; + } else { + _targetRot = 100; + } putMessage(velIn, _targetVel); // Print it out break; @@ -527,11 +512,10 @@ _pComm = comm; _RUN = true; - MINPWM_PRD = 920; + MINPWM_PRD = 920; _tMotorCtrl.start(callback(this, &Motor::motorCtrlFn)); // Start motor control thread - _pComm->_pc.printf("origin=%i, _theStates=[%i,%i,%i]\n\r", _orState, _theStates[0], _theStates[1], _theStates[2]); // Print information to terminal } @@ -655,9 +639,9 @@ bool declared = false; //~~~Controller constants~~~~ - int32_t Kp1=90; // Proportional controller constants - int32_t Kp2=33; // Calculated by trial and error to give optimal accuracy - float Kd=36.5; + int32_t Kp1=88; //22, 27 //Proportional controller constants + int32_t Kp2=26; //12 //Calculated by trial and error to give optimal accuracy + float Kd =34.5; // 40, 14.75 float Kis = 10.0f; // 50 int32_t Kir = 0.0f; float sIntegral = 0.0f; @@ -667,7 +651,8 @@ int32_t Tr; // Initialise controller output Tr (r=rotations) int32_t old_pos = 0, - cur_pos = 0; + cur_pos = 0, + sign = 1; float cur_err = 0.0f, old_err = 0.0f, @@ -694,36 +679,12 @@ while (_RUN) { _tMotorCtrl.signal_wait((int32_t)0x1); - // _pComm->_tCommIn.signal_set(0x01); 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; - // } + cpyModeBitfield = _pComm->_modeBitField; core_util_critical_section_exit(); - - // cur_time = m_timer.read(); // Read state & timestamp - - // 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; - - // Hence we make the value positive,// and instead set the direction to the opposite one - - // 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); - - if ((cpyModeBitfield & 0x01)|(cpyModeBitfield & 0x02)) {// Speed, torque control and PID + if ((cpyModeBitfield & 0x01) | (cpyModeBitfield & 0x02)) {// Speed, torque control and PID if (abs(cur_speed) <= 120 && (!attached)) { eError = 1.0f; attached = true; @@ -735,9 +696,7 @@ 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)) { @@ -760,12 +719,12 @@ } // read state & timestamp - cur_pos = _pComm->_motor_pos; if (cur_pos < 2) { rIntegral = 0.0f; - sIntegral = 0.0f; - declared = false; + sIntegral = 0.0f; + sign = sgn(_pComm->_targetRot); } + cur_pos = (_pComm->_motor_pos)*sign; // USE cur_time = m_timer.read(); // compute speed @@ -802,8 +761,8 @@ } } - Ts = (int32_t)(((Kp1 * sError) + (Kis * sIntegral))); // * sgn(cur_pos)); - Tr = (int32_t)(Kp2*rError + ((Kd/time_diff) * err_diff) + (Kir * rIntegral)); + Ts = (int32_t)(((Kp1 * sError * sign) + (Kis * sIntegral * sign))); // * sgn(cur_pos)); + Tr = (int32_t)((Kp2*rError*sign) + (Kd/time_diff) * err_diff * sign); if (cpyModeBitfield & 0x01) { // Speed control @@ -813,26 +772,24 @@ else { // select minimum absolute value torque - /* - if (cur_speed < 0) { - torque = max(Ts, Tr); - } + // if (cur_speed < 0) { + // torque = max(Ts, Tr); + // } - else { - torque = min(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)); + // _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; + torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque; } } @@ -844,23 +801,15 @@ } 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); + // } - // 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 = Tr; + if (abs(rError) > 0.3) { torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque; } else { torque = 0; @@ -882,12 +831,11 @@ } 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); + _pComm->_pc.printf("RError:%.4f\r\n", rError); // Give the motor a kick @@ -975,15 +923,6 @@ torque = _MAXPWM_PRD * 0.5; // Run at 50% duty cycle if argument not properly defined } - - // 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); - } }