-
Notifications
You must be signed in to change notification settings - Fork 0
/
RampParkF.c
55 lines (48 loc) · 2.42 KB
/
RampParkF.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
#pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, HTServo)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S2, gyro, sensorAnalogInactive)
#pragma config(Sensor, S3, IR, sensorI2CCustom)
#pragma config(Motor, mtr_S1_C1_1, rightDrive, tmotorTetrix, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C1_2, leftDrive, tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor, mtr_S1_C2_1, lift, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C2_2, motorG, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C3_1, arm, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C3_2, bucket, tmotorTetrix, openLoop, encoder)
#pragma config(Servo, srvo_S1_C4_1, dumper, tServoStandard)
#pragma config(Servo, srvo_S1_C4_2, flagMount, tServoStandard)
#pragma config(Servo, srvo_S1_C4_3, flagRaiser, tServoContinuousRotation)
#pragma config(Servo, srvo_S1_C4_4, servo4, tServoNone)
#pragma config(Servo, srvo_S1_C4_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C4_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "JoystickDriver.c" //Include file to "handle" the Bluetooth messages.
#include "hitechnic-irseeker-v2.h" //includes irseeker drivers
#include "gyro.h"//include gyro drivers
#include "pidturn.h"//includes pidturns
#include "drive.h"//includes main library
#include "delay.h"//includes delay library
GYRO g_Gyro;
PIDTURN g_PidTurn;
const int MIN_TURN_POWER = 45;
const float TURN_KP = 0.85;//Default was 0.9
const float TURN_TOLERANCE = 0.3;
void initializeRobot(){
servo[dumper] = 247;
servo[flagMount] = 17;
return;
}
task main()
{
int timeToWait = requestTimeToWait();
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
GyroInit(g_Gyro, gyro, 0);
PidTurnInit(g_PidTurn, leftDrive, rightDrive, MIN_TURN_POWER, g_Gyro, TURN_KP, TURN_TOLERANCE);
//Align against bottom wall, with left edge of left wheels on left edge of third tile (6ft from right wall).
countdown(timeToWait);
moveForwardInches(80, 30, false, LEFTENCODER); //away from wall
turn(g_PidTurn, 90); //turn to parallel with buckets
moveBackwardInches(90,20,false,LEFTENCODER);
while (true)
{}
}