Skad00sh
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Revision 53:89d16b398615, committed 2019-03-22
- Comitter:
- iachinweze1
- Date:
- Fri Mar 22 23:42:55 2019 +0000
- Parent:
- 51:c03f63c6f930
- Child:
- 54:bd5b586aa2e1
- Commit message:
- integrated_1;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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);
-
}
}