-
Notifications
You must be signed in to change notification settings - Fork 0
/
Maze Test Program.c
114 lines (88 loc) · 3.24 KB
/
Maze Test Program.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
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, LeftEncoder, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, RightEncoder, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port1, LeftWheels, tmotorVex269_HBridge, openLoop, driveLeft, encoderPort, I2C_1)
#pragma config(Motor, port2, Arm1, tmotorServoStandard, openLoop)
#pragma config(Motor, port3, Arm2, tmotorServoStandard, openLoop)
#pragma config(Motor, port4, Arm3, tmotorServoStandard, openLoop)
#pragma config(Motor, port5, Claw, tmotorServoStandard, openLoop)
#pragma config(Motor, port10, RightWheels, tmotorVex269_HBridge, openLoop, reversed, driveRight, encoderPort, I2C_2)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#define abs(X) ((X < 0) ? -1 * X : X)
//void init()
//{
// // Just wait for fun
// wait1Msec(3000);
// SensorValue[I2C_1] = 0;
// SensorValue[I2C_2] = 0;
//}
//void driveStraightDistance(int tenthsOfIn, int masterPower)
//{
// int tickGoal = (42 * tenthsOfIn) / 10;
// //This will count up the total encoder ticks despite the fact that the encoders are constantly reset.
// int totalTicks = 0;
// //Initialise slavePower as masterPower - 5 so we don't get huge error for the first few iterations. The
// //-5 value is based off a rough guess of how much the motors are different, which prevents the robot from
// //veering off course at the start of the function.
// int slavePower = masterPower - 5;
// int error = 0;
// int kp = 5;
// SensorValue[I2C_1] = 0;
// SensorValue[I2C_2] = 0;
// //Monitor 'totalTicks', instead of the values of the encoders which are constantly reset.
// while(abs(totalTicks) < tickGoal)
// {
// //Proportional algorithm to keep the robot going straight.
// motor[LeftWheels] = masterPower;
// motor[RightWheels] = slavePower;
// error = SensorValue[I2C_1] - SensorValue[I2C_2];
// slavePower += error / kp;
// SensorValue[I2C_1] = 0;
// SensorValue[I2C_2] = 0;
// wait1Msec(100);
// //Add this iteration's encoder values to totalTicks.
// totalTicks += SensorValue[I2C_1];
// }
// motor[LeftWheels] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks.
// motor[RightWheels] = 0;
//}
//void driveDistance(int distance) {
//}
///**
// * Performs a point turn by rotating both motors the same distance but in opposite directions.
// * Negative values for degress indicate counter-clockwise turns.
// */
//void pointTurn(int degrees) {
//}
///**
// * Performs a pivot turn by locking one wheel in place and rotating the other one.
// * Negative values for degress indicate counter-clockwise turns.
// */
//void pivotTurn(int degrees) {
//}
task main()
{
//init();
// Path 1
// driveStraightDistance(10, 127);
// Path 2
motor[port1] = 127;
motor[port10] = 127;
wait1Msec(3000);
// Path 3
motor[port1] = 127;
motor[port10] = 127;
wait1Msec(3000);
// Path 4
motor[port1] = 127;
motor[port10] = 127;
wait1Msec(3000);
// Path 5
motor[port1] = 127;
motor[port10] = 127;
wait1Msec(2000);
// Path 6
motor[port1] = 127;
motor[port10] = 127;
wait1Msec(1000);
}