Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Diff: main.cpp
- Revision:
- 52:8339cd5c6105
- Parent:
- 46:b9081aa50bda
diff -r b9081aa50bda -r 8339cd5c6105 main.cpp --- a/main.cpp Wed Mar 20 22:31:15 2019 +0000 +++ b/main.cpp Fri Mar 22 13:40:45 2019 +0000 @@ -53,7 +53,7 @@ #define MCSNpin A0 // "Lacros" for utility -#define sgn(x) ((x)/abs(x)) +#define sgn(x) ((x)>=0?1:-1) #define max(x,y) ((x)>=(y)?(x):(y)) #define min(x,y) ((x)>=(y)?(y):(x)) @@ -95,13 +95,14 @@ const uint8_t _MAXCMDLENGTH; // volatile uint8_t _inCharIndex, _cmdIndex; // volatile uint32_t _motorTorque; // Motor Toque + volatile int32_t _motor_pos; volatile uint64_t _newKey; // hash key Mutex _newKeyMutex; // Restrict access to prevent deadlock. RawSerial _pc; Thread _t_comm_out; bool _RUN; - + enum msgType { motorState, posIn, velIn, posOut, velOut, hashRate, keyAdded, nonceMatch, torque, rotations, melody, @@ -148,6 +149,8 @@ _targetVel = 45.0; _targetRot = 459.0; + _motor_pos = 0; + _modeBitField = 0x01; // Default is velocity mode } @@ -201,12 +204,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 + 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; @@ -379,6 +385,8 @@ volatile int8_t currentState; //Current Rotor State volatile int8_t stateList[6]; //All possible rotor states stored + int8_t old_rotor_state; + //Phase lead to make motor spin volatile int8_t lead; @@ -395,10 +403,11 @@ Thread t_motor_ctrl; // Thread for motor Control uint32_t MAXPWM_PRD; + uint32_t MINPWM_PRD; public: - Motor() : t_motor_ctrl(osPriorityAboveNormal2, 1024) + Motor() : t_motor_ctrl(osPriorityAboveNormal2, 2048) { // Set Power to maximum to drive motorHome() dutyC = 1.0f; @@ -406,8 +415,9 @@ pwmCtrl.period_us(mtrPeriod); pwmCtrl.pulsewidth_us(mtrPeriod); - orState = motorHome(); //Rotot offset at motor state 0 - currentState = readRotorState(); //Current Rotor State + orState = motorHome(); // Rotor offset at motor state 0 + currentState = readRotorState(); // Current Rotor State + old_rotor_state = orState; // Set old_rotor_state to the origin to begin with // stateList[6] = {0,0,0, 0,0,0}; //All possible rotor states stored lead = 2; //2 for forwards, -2 for backwards @@ -425,6 +435,32 @@ } + int findMinTorque() { + int8_t prevState = readRotorState(); + uint32_t mtPeriod = 700; + Timer testTimer; + testTimer.start(); + + p_comm->_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 = mtrPeriod >= 1000 ? 700 : mtPeriod; + } + + p_comm->_pc.printf("Min Torque:%i\n", mtPeriod); + return mtPeriod; + } + void motorStart(Comm *comm) { @@ -439,12 +475,15 @@ // push digitally so if motor is static it will start moving motorOut((currentState-orState+lead+6)%6); // We push it digitally - // Default a lower duty cylce + // Default a lower duty cycle dutyC = 0.8; pwmCtrl.period_us((uint32_t)mtrPeriod); pwmCtrl.pulsewidth_us((uint32_t)mtrPeriod*dutyC); p_comm = comm; + // TODO: Make a better findMinTorque() function + // MINPWM_PRD = findMinTorque(); + MINPWM_PRD = 920; _RUN = true; // Start motor control thread @@ -488,7 +527,7 @@ motorOut(0); wait(3.0); - //Get the rotor state + //Get the rotor state return readRotorState(); } @@ -496,18 +535,19 @@ void stateUpdate() { // () { // **params currentState = readRotorState(); - // Store into state counter - if (currentState == theStates[0]) - stateCount[0]++; - else if (currentState == theStates[1]) - stateCount[1]++; - else if (currentState == theStates[2]) - stateCount[2]++; - - // (Current - Offset + lead + 6) %6 motorOut((currentState - orState + lead + 6) % 6); + if (currentState - old_rotor_state == 5) { + p_comm->_motor_pos--; + } else if (currentState - old_rotor_state == -5) { + p_comm->_motor_pos++; + } else { + p_comm->_motor_pos += (currentState - old_rotor_state); + } + + old_rotor_state = currentState; + } @@ -528,35 +568,35 @@ int32_t totalDegrees; int32_t stateDiff; - int32_t cur_speed; //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 volatile int32_t torque; //Local variable to set motor torque - static int32_t oldTorque =0; + static int32_t oldTorque = 0; 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 //~~~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=80; //22, 27 //Proportional controller constants + int32_t Kp2=33; //12 //Calculated by trial and error to give optimal accuracy + float Kis = 0.0f; // 50 + int32_t Kir = 0.0f; + float sIntegral = 0.0f; + float rIntegral = 0.0f; + float Kd =28.5; // 40, 14.75 int32_t Ys; //Initialise controller output Ys (s=speed) int32_t Yr; //Initialise controller output Yr (r=rotations) - - int32_t old_pos = 0; - - uint32_t cur_time = 0, - old_time = 0, - time_diff; + + int32_t old_pos = 0, + cur_pos = 0; float cur_err = 0.0f, old_err = 0.0f, - err_diff; + err_diff, + time_diff, + cur_time = 0.0f, + old_time = 0.0f, + cur_speed = 0.0f; //Variable for local velocity calculation; m_timer.start(); @@ -567,110 +607,163 @@ cpyModeBitfield = p_comm->_modeBitField; // p_comm->_modeBitField = 0; // nah //Access shared variables here - std::copy(stateCount, stateCount+3, cpyStateCount); + /* std::copy(stateCount, stateCount+3, cpyStateCount); cpyCurrentState = currentState; for (int i = 0; i < 3; ++i) { stateCount[i] = 0; - } + } */ + + // p_comm->_pc.printf("R_Error: %f\n", rError); core_util_critical_section_exit(); + // p_comm->_pc.printf("BitField:%#04x\n", cpyModeBitfield); + // read state & timestamp + cur_pos = p_comm->_motor_pos; + if (cur_pos < 2) { + rIntegral = 0.0f; + sIntegral = 0.0f; + } cur_time = m_timer.read(); // compute speed time_diff = cur_time - old_time; - // cur_speed = (cur_pos - old_pos) / time_diff; + cur_speed = (cur_pos - old_pos) / time_diff; // prep values for next time through loop old_time = cur_time; - old_pos = cpyCurrentState; - + old_pos = cur_pos; - iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount; - + // Position error + rError = (p_comm->_targetRot) - (cur_pos/6.0f); + if ((cur_speed != 0)) { + rIntegral += rError * time_diff; + } + err_diff = rError - old_err; + old_err = rError; - totalDegrees = ting[0] * cpyStateCount[iterElementMax]; - stateDiff = theStates[iterElementMax]-cpyCurrentState; - if (stateDiff >= 0) { - totalDegrees = totalDegrees + (ting[1]* stateDiff); - } + // 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 = (p_comm->_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; + } + + Ys = (int32_t)(((Kp1 * sError) + (Kis * sIntegral))); // * sgn(cur_pos)); + Yr = (int32_t)(Kp2*rError + ((Kd/time_diff) * err_diff) + (Kir * rIntegral)); - else { - totalDegrees = totalDegrees + (ting[1]*stateDiff*-1); - } - //p_comm->_pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10)); - - if ((cpyModeBitfield & 0x01) | (cpyModeBitfield & 0x02)) { - //~~~~~Speed controller~~~~~~ - cur_speed = totalDegrees / time_diff; - sError = (p_comm->_targetVel * 6) - abs(cur_speed); //Read global variable _targetVel updated by interrupt and calculate error between target and reality + if (cpyModeBitfield & 0x01) { + // Speed control + if (p_comm->_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(Ys, Yr); + } else { + torque = min(Ys, Yr); + } - if (sError == -abs(cur_speed)) { //Check if user entered V0, - Ys = MAXPWM_PRD; //and set the output to maximum as specified - } + torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque; + // torque = Ys; + } + } else if (cpyModeBitfield & 0x02) { + // select minimum absolute value torque + if (p_comm->_targetRot == 0) { + // Not spinning at max speed + torque = 0.7 * MAXPWM_PRD; + } else { + /* + // TODO: Handle sign or Yr for negative rotations + if (Yr > MINPWM_PRD || Yr < 100) { + torque = Yr; + } else if (Yr >= 100 && Yr <= MINPWM_PRD) { + torque = MINPWM_PRD; + } */ - 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 + // select minimum absolute value torque + if (cur_speed < 0) { + torque = max(Ys, Yr); + } else { + torque = min(Ys, Yr); + } - // } else if (cpyModeBitfield & 0x02) { - //~~~~~Rotation control~~~~~~ - rError = (p_comm->_targetRot)*6 - totalDegrees; //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 (abs(rError) > 0.8) { + // torque = abs(Yr) < 1000 ? 1000*sgn(Yr) : Yr; + torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque; + } else { + torque = 0; + } + + } + } + + // iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount; - Ys = Ys * sgn(rError); - // select minimum absolute value torque - if (cur_speed < 0){ - torque = max(Ys, Yr); - } - else{ - torque = min(Ys, Yr); + /* + // if ((cpyModeBitfield & 0x01) | (cpyModeBitfield & 0x02)) { + if (cpyModeBitfield & 0x01) { + // Speed error - Convert curr_speed from states per time to rotations per time + sError = (p_comm->_targetVel) - (abs(cur_speed/6.0f)); //Read global variable _targetVel updated by interrupt and calculate error between target and reality + + //~~~~~Speed control~~~~~~ + if (p_comm->_targetVel == 0) { //Check if user entered V0, + torque = MAXPWM_PRD; //and set the output to maximum as specified + } else { + Ys = (int32_t)((Kp1 * sError) + Ki*(sError * time_diff)) * sgn(cur_pos); //If the user didn't enter V0 implement controller transfer function: Ys = Kp * (s -|v|) where, + torque = Ys; } - if (torque < 0){ //Variable torque cannot be negative since it sets the PWM - torque = -torque; lead = -2; - } //Hence we make the value positive, - else{ //and instead set the direction to the opposite one - lead = 2; + if (abs(sError) < 3) { + // torque = torque; + p_comm->_pc.printf("Final Speed:%f\n", cur_speed); + } + } else if (cpyModeBitfield & 0x02) { + //~~~~~Rotation control~~~~~~ + if (p_comm->_targetRot == 0) { + // Not spinning at max speed + torque = 0.7 * MAXPWM_PRD; + } else { + Yr = Kp2*rError + (Kd/time_diff) * err_diff; + torque = abs(rError) < 3 ? 0 : Yr; } - - 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->_motorTorque = torque; - pwmCtrl.pulsewidth_us(p_comm->_motorTorque); } if (cpyModeBitfield & 0x04) { // if it is in torque mode, do no math, just set pulsewidth torque = (int32_t)p_comm->_motorTorque; if (oldTorque != torque) { - 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 + 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. + 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->_motorTorque = torque; pwmCtrl.pulsewidth_us(torque); oldTorque = torque; } - } - //else { // if not Torque mode - //balls - //} - // pwmCtrl.write((float)(p_comm->_motorTorque/MAXPWM_PRD)); - // p_comm->_motorTorque = torque; //Lastly, update global variable _motorTorque which is updated by interrupt - // p_comm->_pc.printf("\t\t\t\t\t\t %i, %i, %i \r", torque, Ys, Yr); - //p_comm->_pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10)); + } */ + + if (torque < 0) { + torque = -torque; + lead = -2; + } else { + lead = 2; + } + + torque = torque > MAXPWM_PRD ? MAXPWM_PRD : torque; // Set a cap on torque + + p_comm->_motorTorque = torque; + pwmCtrl.pulsewidth_us(torque); + p_comm->_pc.printf("RError:%.4f, SError:%.4f, CPos:%i, CSpeed:%f, Torque:%i, SInt:%f\r\n", rError, sError, cur_pos, (cur_speed/6.0f), torque, sIntegral); + + // Give the motor a kick + stateUpdate(); } }