Commit dcb02828 authored by Torbjørn Ludvigsen's avatar Torbjørn Ludvigsen 👷

Squashed 'firmware/Mechaduino_subtree/' changes from 30d9a5b35..c24373a51

c24373a51 Lowers power if we're very far from set position.

git-subtree-dir: firmware/Mechaduino_subtree
git-subtree-split: c24373a511fe562bd43c578c039e54630ebd6135
parent 68348f60
......@@ -25,6 +25,21 @@ void TC5_Handler() { // Gets called with FPID frequency
else if (ITerm < -150.0) ITerm = -150.0;
DTerm = pLPFa*DTerm - pLPFb*pKd*(yw-yw_1);
u = (pKp * e) + ITerm + DTerm;
if(fabs(e) > 1800.0){ // We're faaar off. Someone might be stuck. Lower power for safety.
if(fabs(e) < 3600.0){
if(u > 0.0){
u = 2.0*uMAX/3.0;
} else {
u = -2.0*uMAX/3.0;
}
} else { // We're even further off. Lower more.
if(u > 0.0){
u = uMAX/2.0;
} else {
u = -uMAX/2.0;
}
}
}
break;
case 'v': // Velocity control
v = vLPFa*v + vLPFb*(yw-yw_1); // Filtered velocity called "DTerm" because it is similar to derivative action in position loop
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment