forked from Pokealimit/MA4012-Competition
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain_competition.c
88 lines (77 loc) · 3.58 KB
/
main_competition.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
#pragma config(UART_Usage, UART1, uartUserControl, baudRate9600, IOPins, None, None)
#pragma config(Sensor, in2, rightIRSensor, sensorAnalog)
#pragma config(Sensor, in3, leftIRSensor, sensorAnalog)
#pragma config(Sensor, in4, backIRSensor, sensorAnalog)
#pragma config(Sensor, in5, back_limit_1, sensorAnalog)
#pragma config(Sensor, in6, back_limit_2, sensorAnalog)
#pragma config(Sensor, in7, topIRSensor, sensorAnalog)
#pragma config(Sensor, in8, back_limit_4, sensorAnalog)
#pragma config(Sensor, dgtl1, Power_Switch, sensorDigitalIn)
#pragma config(Sensor, dgtl2, leftWheelSensor, sensorDigitalIn)
#pragma config(Sensor, dgtl3, rightWheelSensor, sensorDigitalIn)
#pragma config(Sensor, dgtl4, ball_limit_switch, sensorDigitalIn)
#pragma config(Sensor, dgtl5, back_limit_3, sensorDigitalIn)
#pragma config(Sensor, dgtl6, leftLF, sensorDigitalIn)
#pragma config(Sensor, dgtl7, rightLF, sensorDigitalIn)
#pragma config(Sensor, dgtl8, compass_LSB, sensorDigitalIn)
#pragma config(Sensor, dgtl9, compass_Bit3, sensorDigitalIn)
#pragma config(Sensor, dgtl10, compass_Bit2, sensorDigitalIn)
#pragma config(Sensor, dgtl11, compass_MSB, sensorDigitalIn)
#pragma config(Motor, port2, leftMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, rightMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port4, roller, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor, port5, servo, tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "var_competition.c"
#include "encoder_competition.c"
task main(){
//Initialise variables and start multi-tasking for encoder tasks
initialise();
setBaudRate(UART1, baudRate9600);
startTask(counting);
startTask(mapping);
while(true){
//writeDebugStreamLine("Battery: %d",nAvgBatteryLevel);
//writeDebugStreamLine("left distance = %f", leftSensorReading());
//writeDebugStreamLine("right distance = %f", rightSensorReading());
//delay(500);
while (SensorValue(Power_Switch) == Power_Switch_ON && boundary_avoidance()){
//main code here:
if (first_launch) {
if(move_straight_with_check(160)) first_launch = false;
delay(500);
if (pan_and_search_without_overpan(180, 'r') == 0 || SensorValue(Power_Switch) == Power_Switch_OFF) break;
delay(500);
if (pan_and_search_without_overpan(180, 'r') == 0 || SensorValue(Power_Switch) == Power_Switch_OFF) break;
delay(500);
}
if (next_launch) {
if(move_straight_with_check(60)) next_launch = false;
}
if (phase_change) phase_change = false;
if (move_straight_with_check(60)==0 || SensorValue(Power_Switch) == Power_Switch_OFF) break;
delay(500);
if (pan_and_search(180, 'r') == 0 || SensorValue(Power_Switch) == Power_Switch_OFF) break;
delay(500);
if (phase==180){
if (pan_and_search(180, 'l') == 0 || SensorValue(Power_Switch) == Power_Switch_OFF) break;
delay(500);
}
else{
if (pan_and_search(180, 'r') == 0 || SensorValue(Power_Switch) == Power_Switch_OFF) break;
delay(500);
}
if(phase==270 && round_count%2==1){
phase -= 90;
}
pan_to_heading(phase); // Reorientate back to the phase as pan_by_degree is not extremely accurate
}
//If power is off, stop all motors and reinitalise variables
if(SensorValue(Power_Switch) == Power_Switch_OFF){
moveMotor(0,0,'f',0);
motor(roller)=0;
motor(servo)=-127;
initialise();
}
}
}