Pastebin launched a little side project called VERYVIRAL.com, check it out ;-) Want more features on Pastebin? Sign Up, it's FREE!
Guest

SoftBumpTest

By: a guest on Jan 6th, 2012  |  syntax: C  |  size: 3.33 KB  |  views: 6  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1.  
  2. #pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
  3. #pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
  4. #pragma config(Sensor, S2,     HTGYRO,              sensorI2CHiTechnicGyro)
  5. #pragma config(Sensor, S3,     HTIRS1,              sensorI2CCustom)
  6. #pragma config(Sensor, S4,     HTIRS2,              sensorI2CCustom)
  7. #pragma config(Motor,  mtr_S1_C1_1,     Ldrive,        tmotorNormal, PIDControl, reversed, encoder)
  8. #pragma config(Motor,  mtr_S1_C1_2,     Rdrive,        tmotorNormal, PIDControl, encoder)
  9. #pragma config(Motor,  mtr_S1_C2_1,     BallVac,       tmotorNormal, openLoop)
  10. #pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
  11. #pragma config(Motor,  mtr_S1_C3_1,     motorH,        tmotorNormal, PIDControl, encoder)
  12. #pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorNormal, PIDControl, encoder)
  13. #pragma config(Servo,  srvo_S1_C4_1,    LeftGate,             tServoStandard)
  14. #pragma config(Servo,  srvo_S1_C4_2,    RightGate,            tServoStandard)
  15. #pragma config(Servo,  srvo_S1_C4_3,    CrateFlip,            tServoStandard)
  16. #pragma config(Servo,  srvo_S1_C4_4,    Gate,                 tServoStandard)
  17. #pragma config(Servo,  srvo_S1_C4_5,    RBC,                  tServoStandard)
  18. #pragma config(Servo,  srvo_S1_C4_6,    RFC,                  tServoStandard)
  19. //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
  20.  
  21. #include "C:\Users\AlecG\Desktop\Robot-C\Includes\auxiliaryfunctions.h"
  22. #include "C:\Users\AlecG\Desktop\Robot-C\Includes\common.h"
  23. #include "C:\Users\AlecG\Desktop\Robot-C\Includes\HTIRS2-driver.h"
  24. #include "C:\Users\AlecG\Desktop\Robot-C\rdpartyrobotcdr-v1.8.1-HT\drivers\HTGYRO-driver.h"
  25. //D is left drive E is Right
  26.  
  27.  
  28. float speedValue = 0;
  29.  
  30.  
  31.  
  32.  ///////////////////////////////////////////////////////////////////////////////////////////////
  33. // This whole function is designed to move the robot a defined distance precisely. It also converts inches to encoder counts.
  34. void MoveBump(int inDist, int power)
  35. {
  36.  float EncoderCounts = (inDist - 1.9364)/0.0064; //This is all the data that Alec took last year put into an equation.
  37.  inDist = EncoderCounts; // Floats screw up all the things. so we used inDist but made it hold the encoder value.
  38.  //writeDebugStreamLine( "%d",inDist );
  39.  while(abs(nMotorEncoder[motorE]) < inDist && abs(nMotorEncoder[motorD]) < inDist)
  40.  {
  41.    writeDebugStreamLine("%f" ,SpeedCheck(Ldrive, 0));
  42.    motor[Ldrive] = power;
  43.    motor[Rdrive] = power;
  44.  }
  45.  motor[Ldrive] = 0;
  46.  motor[Rdrive] = 0;
  47.  wait10Msec(1); // just to let the encoders reset when the robot is still
  48.  nMotorEncoder[Ldrive] = 0;
  49.  nMotorEncoder[Rdrive] = 0;
  50. }
  51.  
  52.  
  53. /* Function returns the speed of a Tetrix DC motor equipped with encoder in RPMs.
  54.  
  55. Inputs to the function are the motor you want to check, and an index.  Use a different index for every
  56. motor you are checking.
  57.  
  58. */
  59.  
  60.  task main()
  61.  {
  62.    while(true)
  63.    {
  64.      motor[Ldrive] = 50;
  65.      motor[Rdrive] = 50;
  66.      speedValue = SpeedCheck(Ldrive, 0);
  67.      if (speedValue != 0)
  68.      {
  69.        writeDebugStreamLine ("%f", speedValue);
  70.      }
  71.      //writeDebugStreamLine("%f" ,SpeedCheck(Ldrive, 0));
  72.      //MoveBump(100, 50);// this is the speed (50) of our Blue and Red 20 autonomous programs
  73.      //writeDebugStream("alec");
  74.      wait1Msec(10);
  75.    }
  76.  }