API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Feb 16th, 2020
66
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 9.51 KB | None | 0 0
raw download clone embed print report
  1.  
  2. #include "321Hlib/SystemControl/driveTask.h"
  3.  
  4. /**************************************************/
  5. //Drive State Machine Methods
  6. static driveStates currentdriveState = driveStates::tank; //Defaulted tank
  7. double drivePercent = 1.0;
  8.  
  9. void setdriveState(driveStates newState){ //Set State of drive
  10. currentdriveState = newState;
  11. }
  12. void setdriveState(driveStates newState, double requestedPercent){
  13. currentdriveState = newState;
  14. drivePercent = requestedPercent;
  15. }
  16. /**************************************************/
  17.  
  18. bool drivePIDOn = false;
  19. double kpDrive = 1;
  20. double kdDrive = .1;
  21. static double driveTarget;
  22.  
  23. bool turnPIDOn = false;
  24. double kp = 1.25;//.7
  25. double kd = .1;//.1
  26. static double turnTarget;
  27.  
  28. static double maxSpeed = 200;
  29. static int yaw = 0; //Used for arcing
  30.  
  31. double getImuRotation(){ //()-infinity, infinity)
  32. return s_imu.get_rotation();
  33. }
  34.  
  35. double getEncRotation(){
  36. return m_driveRB.getPosition();
  37. }
  38.  
  39. double getDrivePos(){
  40. return (m_driveRB.getPosition() + m_driveLB.getPosition())/2;
  41. }
  42. double inchesToTicks(double inches){
  43. double rev = inches/3.1415/4;
  44. double ticks = rev * 360;
  45. return ticks;
  46. }
  47.  
  48. void reset(){
  49. turnTarget = 0;
  50. driveTarget = 0;
  51. yaw = 0;
  52. maxSpeed = 200;
  53. m_driveLF.tarePosition();
  54. m_driveLB.tarePosition();
  55. m_driveRF.tarePosition();
  56. m_driveRB.tarePosition();
  57. mg_driveR.moveVelocity(0);
  58. mg_driveL.moveVelocity(0);
  59. }
  60. //
  61. // //slew control
  62. // const int accel_step = 9;
  63. // const int deccel_step = 256; // no decel slew
  64. // static int leftSpeed = 0;
  65. // static int rightSpeed = 0;
  66. //
  67. // void leftSlew(int leftTarget){
  68. // int step;
  69. //
  70. // if(abs(leftSpeed) < abs(leftTarget))
  71. // step = accel_step;
  72. // else
  73. // step = deccel_step;
  74. //
  75. // if(leftTarget > leftSpeed + step)
  76. // leftSpeed += step;
  77. // else if(leftTarget < leftSpeed - step)
  78. // leftSpeed -= step;
  79. // else
  80. // leftSpeed = leftTarget;
  81. //
  82. // mg_driveL.moveVoltage(leftSpeed);
  83. // }
  84. //
  85. // void rightSlew(int rightTarget){
  86. // int step;
  87. //
  88. // if(abs(rightSpeed) < abs(rightTarget))
  89. // step = accel_step;
  90. // else
  91. // step = deccel_step;
  92. //
  93. // if(rightTarget > rightSpeed + step)
  94. // rightSpeed += step;
  95. // else if(rightTarget < rightSpeed - step)
  96. // rightSpeed -= step;
  97. // else
  98. // rightSpeed = rightTarget;
  99. //
  100. // mg_driveR.moveVoltage(rightSpeed);
  101. // }
  102.  
  103. void setMaxSpeed(int speed){
  104. maxSpeed = speed;
  105. }
  106. bool isTurning_encoder(){
  107.  
  108. static int count = 0;
  109. static double lastPos = 0;
  110. static double lastTarget = 0;
  111.  
  112. double curPos = getEncRotation();
  113. double target = turnTarget;
  114. if(currentdriveState == driveStates::drivePID){
  115. target = driveTarget;
  116. }
  117.  
  118. if(abs(lastPos-curPos) < 3){ //3 robot degree threshold for change in Angle NEEDS TUNING
  119. count++;
  120. }
  121. else{
  122. count = 0;
  123. }
  124.  
  125. if(target != lastTarget){ //If changing target, restart iterating
  126. count = 0;
  127. }
  128.  
  129. // std::cout << "lastAngle: " << lastAngle << std::endl;
  130. // std::cout << "curAngle: " << curAngle << std::endl;
  131. // std::cout << "count: " << count << std::endl;
  132.  
  133. lastTarget = target;
  134. lastPos = curPos;
  135.  
  136.  
  137. if(count > 4){ //If iterated 5 times
  138. return false; //Not Turning
  139. }
  140. else{
  141. return true; //Turning
  142. }
  143. }
  144.  
  145. bool isTurning(){
  146.  
  147. static int count = 0;
  148. static double lastPos = 0;
  149. static double lastTarget = 0;
  150.  
  151. double curPos = getImuRotation();
  152. double target = turnTarget;
  153. if(currentdriveState == driveStates::drivePID){
  154. target = driveTarget;
  155. }
  156.  
  157. if(abs(lastPos-curPos) < .05){ //3 robot degree threshold for change in Angle NEEDS TUNING
  158. count++;
  159. }
  160. else{
  161. count = 0;
  162. }
  163.  
  164. if(target != lastTarget){ //If changing target, restart iterating
  165. count = 0;
  166. }
  167.  
  168. // std::cout << "lastAngle: " << lastAngle << std::endl;
  169. // std::cout << "curAngle: " << curAngle << std::endl;
  170. // std::cout << "count: " << count << std::endl;
  171.  
  172. lastTarget = target;
  173. lastPos = curPos;
  174.  
  175.  
  176. if(count > 4){ //If iterated 5 times
  177. return false; //Not Turning
  178. }
  179. else{
  180. return true; //Turning
  181. }
  182. }
  183.  
  184. void turnTo(double newHeading){ //Heading in degrees (0,360)
  185. reset();
  186.  
  187. // turnTarget = headingToTarget(newHeading);
  188. turnTarget = newHeading;
  189. setdriveState(driveStates::turnPID);
  190. turnPIDOn = true;
  191.  
  192. pros::delay(300); //Assuming no super short turns
  193. while(isTurning()){ //Wait until settled
  194. setdriveState(driveStates::turnPID);
  195. pros::delay(20);
  196. }
  197. //Movement over
  198. setdriveState(driveStates::tank);
  199. //std::cout << "turnTo Over!!" << std::endl;
  200. turnPIDOn = false;
  201. }
  202.  
  203. void drive(double distance, int speed){ //Inchess
  204. reset();
  205. setMaxSpeed(speed);
  206. driveTarget = inchesToTicks(distance);
  207. setdriveState(driveStates::drivePID);
  208. drivePIDOn = true;
  209. pros::delay(300);
  210. while(isTurning()){
  211. setdriveState(driveStates::drivePID);
  212. pros::delay(20);
  213. }
  214. setdriveState(driveStates::tank);
  215. drivePIDOn = false;
  216. }
  217.  
  218. void drive(double distance, int speed, int yawInput){//Arc
  219. reset();
  220. yaw = yawInput;
  221. setMaxSpeed(speed);
  222. driveTarget = inchesToTicks(distance);
  223. setdriveState(driveStates::drivePID);
  224. drivePIDOn = true;
  225. pros::delay(300);
  226. while(isTurning()){
  227. setdriveState(driveStates::drivePID);
  228. pros::delay(20);
  229. }
  230. setdriveState(driveStates::tank);
  231. drivePIDOn = false;
  232. }
  233.  
  234.  
  235.  
  236. //Two speed drive
  237. void variableDrive(double distance, double dropDistance, int vi, int vf){
  238. reset();
  239.  
  240. driveTarget = inchesToTicks(distance);
  241. int dropDistanceCP = inchesToTicks(dropDistance);
  242. setdriveState(driveStates::drivePID);
  243. drivePIDOn = true;
  244.  
  245. //Drive at this speed until this point
  246. setMaxSpeed(vi);
  247. if(distance > 0){
  248. while(getDrivePos() < dropDistanceCP){
  249. pros::delay(20);
  250. setdriveState(driveStates::drivePID);
  251. }
  252. }
  253. else{
  254. while(getDrivePos() > dropDistanceCP) {
  255. pros::delay(20);
  256. setdriveState(driveStates::drivePID);
  257. }
  258. }
  259.  
  260. //For 4 iterations, drive forward at high speed, slow down.. would need to implement slew control
  261. //for(int i = 0; i < 4; i++){
  262. //}
  263.  
  264. //Drive at a slower speed
  265. setMaxSpeed(vf);
  266. while(isTurning()) {
  267. setdriveState(driveStates::drivePID);
  268. pros::delay(20);
  269. }
  270. }
  271.  
  272. /**************************************************/
  273. //State Machine Task for Drive Control
  274. void task_driveControl(void*){
  275. while(true){
  276.  
  277. switch(currentdriveState){
  278.  
  279. case driveStates::tank:{
  280. if(mg_driveR.getBrakeMode() == AbstractMotor::brakeMode::hold){ //Change from Hold to Coast
  281. mg_driveR.setBrakeMode(AbstractMotor::brakeMode::coast);
  282. mg_driveL.setBrakeMode(AbstractMotor::brakeMode::coast);
  283. }
  284. pidChassis->getModel()->tank(j_master.getAnalog(ControllerAnalog::leftY)*drivePercent,
  285. j_master.getAnalog(ControllerAnalog::rightY)*drivePercent);//Add threshold?
  286. break;
  287. }
  288.  
  289. case driveStates::hold:{
  290. mg_driveR.setBrakeMode(AbstractMotor::brakeMode::hold);
  291. mg_driveL.setBrakeMode(AbstractMotor::brakeMode::hold);
  292. mg_driveR.moveVelocity(0);
  293. mg_driveL.moveVelocity(0);
  294. break;
  295. }
  296. case driveStates::outOfStack:{
  297. mg_driveR.setBrakeMode(AbstractMotor::brakeMode::coast);
  298. mg_driveL.setBrakeMode(AbstractMotor::brakeMode::coast);
  299. setintakeState(intakeStates::on,-12000);
  300. mg_driveR.moveVelocity(-30);
  301. mg_driveL.moveVelocity(-30);
  302. }
  303. case driveStates::drivePID:{
  304. int prevError;
  305.  
  306. while(drivePIDOn){
  307. double target = driveTarget;
  308. double current = getEncRotation();
  309.  
  310. double error = target-current;
  311. double derivative = error - prevError;
  312. prevError = error;
  313.  
  314. double speed = error*kp + derivative*kd;
  315.  
  316. if(speed > maxSpeed)
  317. speed = maxSpeed;
  318. if(speed < -maxSpeed)
  319. speed = -maxSpeed;
  320.  
  321. // std::cout << "turnTarget: " << target << std::endl;
  322. // std::cout << "rotation: " << current << std::endl;
  323. // std::cout << "error: " << error << std::endl;
  324. // std::cout << "speed: " << speed << std::endl;
  325.  
  326.  
  327. mg_driveL.moveVelocity(speed + yaw);
  328. mg_driveR.moveVelocity(speed - yaw);
  329. // leftSlew(-speed);
  330. // rightSlew(speed);
  331. //std::cout << "PID is on!" << std::endl;
  332. pros::delay(20);
  333. }
  334.  
  335. }
  336. case driveStates::turnPID:{
  337. int prevError;
  338.  
  339. while(turnPIDOn){
  340. double target = turnTarget; //Set in turnTo method
  341. double current = getImuRotation();
  342.  
  343. double error = target-current;
  344. double derivative = error - prevError;
  345. prevError = error;
  346.  
  347. double speed = error*kp + derivative*kd;
  348.  
  349. if(speed > maxSpeed)
  350. speed = maxSpeed;
  351. if(speed < -maxSpeed)
  352. speed = -maxSpeed;
  353.  
  354. // std::cout << "turnTarget: " << target << std::endl;
  355. // std::cout << "rotation: " << current << std::endl;
  356. // std::cout << "error: " << error << std::endl;
  357. // std::cout << "speed: " << speed << std::endl;
  358.  
  359.  
  360. mg_driveL.moveVelocity(speed);
  361. mg_driveR.moveVelocity(-speed);
  362. // leftSlew(-speed);
  363. // rightSlew(speed);
  364. //std::cout << "PID is on!" << std::endl;
  365. pros::delay(20);
  366. }
  367.  
  368. }
  369. }
  370. pros::delay(20);
  371. }
  372. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!