Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, HTServo)
- #pragma config(Sensor, S1, , sensorI2CMuxController)
- #pragma config(Sensor, S2, HTGYRO, sensorI2CHiTechnicGyro)
- #pragma config(Sensor, S3, HTIRS1, sensorI2CCustom)
- #pragma config(Sensor, S4, HTIRS2, sensorI2CCustom)
- #pragma config(Motor, mtr_S1_C1_1, Ldrive, tmotorNormal, PIDControl, reversed, encoder)
- #pragma config(Motor, mtr_S1_C1_2, Rdrive, tmotorNormal, PIDControl, encoder)
- #pragma config(Motor, mtr_S1_C2_1, BallVac, tmotorNormal, openLoop)
- #pragma config(Motor, mtr_S1_C2_2, motorG, tmotorNormal, openLoop)
- #pragma config(Motor, mtr_S1_C3_1, motorH, tmotorNormal, PIDControl, encoder)
- #pragma config(Motor, mtr_S1_C3_2, motorI, tmotorNormal, PIDControl, encoder)
- #pragma config(Servo, srvo_S1_C4_1, LeftGate, tServoStandard)
- #pragma config(Servo, srvo_S1_C4_2, RightGate, tServoStandard)
- #pragma config(Servo, srvo_S1_C4_3, CrateFlip, tServoStandard)
- #pragma config(Servo, srvo_S1_C4_4, Gate, tServoStandard)
- #pragma config(Servo, srvo_S1_C4_5, RBC, tServoStandard)
- #pragma config(Servo, srvo_S1_C4_6, RFC, tServoStandard)
- //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
- #include "C:\Users\AlecG\Desktop\Robot-C\Includes\auxiliaryfunctions.h"
- #include "C:\Users\AlecG\Desktop\Robot-C\Includes\common.h"
- #include "C:\Users\AlecG\Desktop\Robot-C\Includes\HTIRS2-driver.h"
- #include "C:\Users\AlecG\Desktop\Robot-C\rdpartyrobotcdr-v1.8.1-HT\drivers\HTGYRO-driver.h"
- //D is left drive E is Right
- float speedValue = 0;
- ///////////////////////////////////////////////////////////////////////////////////////////////
- // This whole function is designed to move the robot a defined distance precisely. It also converts inches to encoder counts.
- void MoveBump(int inDist, int power)
- {
- float EncoderCounts = (inDist - 1.9364)/0.0064; //This is all the data that Alec took last year put into an equation.
- inDist = EncoderCounts; // Floats screw up all the things. so we used inDist but made it hold the encoder value.
- //writeDebugStreamLine( "%d",inDist );
- while(abs(nMotorEncoder[motorE]) < inDist && abs(nMotorEncoder[motorD]) < inDist)
- {
- writeDebugStreamLine("%f" ,SpeedCheck(Ldrive, 0));
- motor[Ldrive] = power;
- motor[Rdrive] = power;
- }
- motor[Ldrive] = 0;
- motor[Rdrive] = 0;
- wait10Msec(1); // just to let the encoders reset when the robot is still
- nMotorEncoder[Ldrive] = 0;
- nMotorEncoder[Rdrive] = 0;
- }
- /* Function returns the speed of a Tetrix DC motor equipped with encoder in RPMs.
- Inputs to the function are the motor you want to check, and an index. Use a different index for every
- motor you are checking.
- */
- task main()
- {
- while(true)
- {
- motor[Ldrive] = 50;
- motor[Rdrive] = 50;
- speedValue = SpeedCheck(Ldrive, 0);
- if (speedValue != 0)
- {
- writeDebugStreamLine ("%f", speedValue);
- }
- //writeDebugStreamLine("%f" ,SpeedCheck(Ldrive, 0));
- //MoveBump(100, 50);// this is the speed (50) of our Blue and Red 20 autonomous programs
- //writeDebugStream("alec");
- wait1Msec(10);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement