Skad00sh
Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Revision 40:9fae84f111e6, committed 2019-03-20
- Comitter:
- adehadd
- Date:
- Wed Mar 20 13:49:57 2019 +0000
- Parent:
- 39:05a021718517
- Child:
- 41:6e730621622b
- Commit message:
- science not really applied, but PID might be better now
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Mar 20 10:01:38 2019 +0000
+++ b/main.cpp Wed Mar 20 13:49:57 2019 +0000
@@ -4,11 +4,13 @@
// #include "rtos.h"
/*TODO:
-Change
-Indx
-newCmd
-MAXCMDLENGTH
+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
*/
//Photointerrupter input pins
@@ -34,6 +36,11 @@
#define MCSPpin A1
#define MCSNpin A0
+// "Lacros" for utility
+#define sgn(x) ((x)/abs(x))
+#define max(x,y) ((x)>=(y)?(x):(y))
+#define min(x,y) ((x)>=(y)?(y):(x))
+
//Mapping from sequential drive states to motor phase outputs
/*
State L1 L2 L3
@@ -231,11 +238,11 @@
pc.printf("\r>%s< Target Velocity set to:\t%.2f\n\r", inCharQ, targetVel);
break;
case velOut:
- pc.printf("\r>%s< Current Velocity:\t%.2f\n\r", inCharQ, \
+ pc.printf("\r>%s< Current Velocity:\t%.2f States/sec\n\r", inCharQ, \
(float) ((int32_t) pMessage->message /*/ 6*/));
break;
case posIn:
- pc.printf("\r>%s< Target Rotation set to:\t%.2f\n\r", inCharQ, \
+ pc.printf("\r>%s< Target # Rotations:\t%.2f\n\r", inCharQ, \
(float) ((int32_t) pMessage->message /*/ 6*/));
break;
case posOut:
@@ -499,6 +506,7 @@
// attach_us -> runs funtion every 100ms
void motorCtrlFn() {
Ticker motorCtrlTicker;
+ Timer m_timer;
motorCtrlTicker.attach_us(callback(this,&Motor::motorCtrlTick), 1e5);
// Init some things
@@ -506,16 +514,17 @@
uint8_t cpyCurrentState;
int8_t cpyModeBitfield;
- int32_t ting[2] = {5,1}; // 360,60 (for degrees), 5,1 (for states)
+ int32_t ting[2] = {6,1}; // 360,60 (for degrees), 5,1 (for states)
uint8_t iterElementMax;
int32_t totalDegrees;
int32_t stateDiff;
- int32_t velocity; //Variable for local velocity calculation
+ 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;
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
@@ -529,21 +538,44 @@
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;
+
+ float cur_err = 0.0f,
+ old_err = 0.0f,
+ err_diff;
+
+ m_timer.start();
+
while (_RUN) {
t_motor_ctrl.signal_wait((int32_t)0x1);
+
core_util_critical_section_enter();
-
cpyModeBitfield = p_comm->modeBitfield;
- p_comm->modeBitfield = 0;
+ // p_comm->modeBitfield = 0; // nah
//Access shared variables here
std::copy(stateCount, stateCount+3, cpyStateCount);
- // TODO: A thing yes
cpyCurrentState = currentState;
for (int i = 0; i < 3; ++i) {
stateCount[i] = 0;
}
core_util_critical_section_exit();
+ // read state & timestamp
+ 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 = cpyCurrentState;
+
+
iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount;
@@ -558,51 +590,35 @@
if ((cpyModeBitfield & 0x01) | (cpyModeBitfield & 0x02)) {
//~~~~~Speed controller~~~~~~
- velocity = totalDegrees*10;
- sError = (p_comm->targetVel * 6) - abs(velocity); //Read global variable targetVel updated by interrupt and calculate error between target and reality
+ 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 (sError == -abs(velocity)) { //Check if user entered V0,
+ if (sError == -abs(cur_speed)) { //Check if user entered V0,
Ys = MAXPWM_PRD; //and set the output to maximum as specified
} 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
-// } else if (cpyModeBitfield & 0x02) {
+ // } else if (cpyModeBitfield & 0x02) {
//~~~~~Rotation control~~~~~~
- rError = p_comm->targetRot - cpyCurrentState; //Read global variable targetRot updated by interrupt and calculate the rotation error.
+ 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(rError < 0) //Use the sign of the error to set controller wrt direction of rotation
+ // Ys = -Ys;
- if (cpyModeBitfield & 0x04) { // if it is in torque mode, do no math, just set pulsewidth
- torque = (int32_t)p_comm->motorPower;
- 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 {
+ Ys = Ys * sgn(rError);
+ // select minimum absolute value torque
+ if (cur_speed < 0)
+ torque = max(Ys, Yr);
+ else
+ torque = min(Ys, Yr);
+
+ 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(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->motorPower = torque;
- pwmCtrl.pulsewidth_us(torque);
- } else { // if not Torque mode
- if((velocity>=0 && Ys<Yr) || (velocity<0 && Ys>Yr)){ //Choose Ys or Yr based on distance from target value so that it takes
- torque = Ys; //appropriate steps in the right direction to reach target value
- } else {
- torque = Yr;
- }
- 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.
}
@@ -610,6 +626,28 @@
p_comm->motorPower = torque;
pwmCtrl.pulsewidth_us(p_comm->motorPower);
}
+
+ if (cpyModeBitfield & 0x04) { // if it is in torque mode, do no math, just set pulsewidth
+ torque = (int32_t)p_comm->motorPower;
+ 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
+ } 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.
+
+ }
+ p_comm->putMessage((Comm::msgType)8, torque);
+ p_comm->motorPower = torque;
+ pwmCtrl.pulsewidth_us(torque);
+ oldTorque = torque;
+ }
+ } else { // if not Torque mode
+ //balls
+ }
// pwmCtrl.write((float)(p_comm->motorPower/MAXPWM_PRD));
// p_comm->motorPower = torque; //Lastly, update global variable motorPower which is updated by interrupt
// p_comm->pc.printf("\t\t\t\t\t\t %i, %i, %i \r", torque, Ys, Yr);