src/auto.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

#include <main.h>

#define TARGET_RPM 1970

static unsigned int time;

void autoDelay(unsigned int ms){
	delay(ms);
	time-=ms;
}

void autonomous(){
	static int    trpm;
	static double rpm;
	static char cs;

	time = 15000;
	cs = rpm = trpm = 0;

	do{

		cs+=cs<100?10:0;

		motorSet(CANNON1,cs);
		motorSet(CANNON2,cs);
		motorSet(CANNON3,cs);
		motorSet(CANNON4,cs);

		autoDelay(800);

		imeGetVelocity(ICANNON3,&trpm);
		rpm=abs(trpm)/24.5L;
		rpm*=25.0L;

	}while(rpm<TARGET_RPM);

	motorSet(LIFT1,127);
	motorSet(LIFT2,127);

	delay(time);

	// Stop										4000ms remaining

	motorStopAll();

}