1. #pragma config(ProgramType, StandaloneWiFi)
  2. //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
  3.  
  4. /*************************************************************************
  5. Robs Robot Configuration
  6.  
  7. Description: This program gives user full control over all four wheels of
  8. the robot.
  9.  
  10. Configuration: This program is written to work with a modified Squarebot model.
  11. - Motors in order Clockwise from battery. (1 - 4)
  12.  
  13. Additional Notes:
  14. -	Robot uses perpendicular motions to move in all directions.
  15. -	Power levels that can be assigned to a motor port range from -127 (full
  16. 	reverse) to 127 (full forward).
  17. *************************************************************************/
  18. int rotateHelper;
  19.  
  20. task main()
  21. {
  22. 	while(abs(vexRT[Ch2]) > abs(vexRT[Ch1])) //abs = absolute value
  23. 	{
  24. 	  // When right Joystick is Up or Down
  25. 	  // Robot goes Forward or Backward
  26.  
  27. 	  while(abs(vexRT[Ch2]) < 87)
  28. 	  {
  29. 	    // If speed is low, increase speeds to rotate.
  30.  
  31. 	    motor[port1] = -vexRT[Ch2] - rotateHelper;
  32. 	    motor[port2] = vexRT[Ch2] + rotateHelper;
  33. 	    motor[port3] = vexRT[Ch2] - rotateHelper;
  34. 	    motor[port4] = -vexRT[Ch2] + rotateHelper;
  35. 	  }
  36. 	  while(abs(vexRT[Ch2]) > 87)
  37. 	  {
  38. 	    // If speed is high, decrease speeds to rotate.
  39.  
  40. 	    motor[port1] = -87 - rotateHelper;
  41. 	    motor[port2] = 87 + rotateHelper;
  42. 	    motor[port3] = 87 - rotateHelper;
  43. 	    motor[port4] = -87 + rotateHelper;
  44. 	  }
  45. 	}
  46. 	while(abs(vexRT[Ch2]) < abs(vexRT[Ch1])) // abs = absolute value
  47. 	{
  48. 	  // When right Joystick is Right or Left
  49. 	  // Robot goes Right or Left
  50.  
  51. 	  while(abs(vexRT[Ch1]) < 87)
  52. 	  {
  53. 	    // If speed is low, increase speeds to rotate.
  54.  
  55. 	    motor[port1] = vexRT[Ch1] - rotateHelper;
  56. 	    motor[port2] = vexRT[Ch1] + rotateHelper;
  57. 	    motor[port3] = -vexRT[Ch1] - rotateHelper;
  58. 	    motor[port4] = -vexRT[Ch1] + rotateHelper;
  59. 	  }
  60. 	  while(abs(vexRT[Ch1]) > 87)
  61. 	  {
  62. 	    // If speed is high, decrease speeds to rotate.
  63.  
  64. 	    motor[port1] = 87 - rotateHelper;
  65. 	    motor[port2] = 87 + rotateHelper;
  66. 	    motor[port3] = -87 - rotateHelper;
  67. 	    motor[port4] = -87 + rotateHelper;
  68. 	  }
  69. 	}
  70.   while(vexRT[Ch6] == 127)
  71.   {
  72.     // If right backbutton up is pressed, set rotateHelper to 40.
  73.  
  74.     rotateHelper = 40;
  75.   }
  76.   while(vexRT[Ch5] == 127)
  77.   {
  78.     // If left backbutton up is pressed, set rotateHelper to -40.
  79.  
  80.     rotateHelper = -40;
  81.   }
  82.   while(vexRT[Ch6] == 0 && vexRT[Ch5] == 0)
  83.   {
  84.     // If backbuttons are not pressed, set rotateHelper to 0.
  85.  
  86.     rotateHelper = 0;
  87.   }
  88. }//end main