Skad00sh
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Revision 52:8339cd5c6105, committed 2019-03-22
- Comitter:
- iachinweze1
- Date:
- Fri Mar 22 13:40:45 2019 +0000
- Parent:
- 46:b9081aa50bda
- Commit message:
- iz nice but speed control is fakked;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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();
}
}