Revision of mecanum_route.c

jesse revised this gist 1 hour ago. Go to revision

1 file changed, 1 insertion

mecanum_route.c

			@@ -1,6 +1,7 @@
1	1		#include <kipr/wombat.h>
2	2		#include <stdlib.h>
3	3		#define BLACK_VALUE 3700
	4	+	//comment
4	5
5	6		int stop_at_line(int min_ticks){
6	7		if(analog(1)>BLACK_VALUE && gmpc(0)> min_ticks){

jesse revised this gist 8 hours ago. Go to revision

1 file changed, 222 insertions

mecanum_route.c(file created)

		@@ -0,0 +1,222 @@
1	+	#include <kipr/wombat.h>
2	+	#include <stdlib.h>
3	+	#define BLACK_VALUE 3700
4	+
5	+	int stop_at_line(int min_ticks){
6	+	if(analog(1)>BLACK_VALUE && gmpc(0)> min_ticks){
7	+	printf("reach end of ramp\n");
8	+	return 1;
9	+	}
10	+	printf("keep going analog_1=%d, gmpc_0=%d\n",analog(1), gmpc(0));
11	+	return 0;
12	+	}
13	+	void follow_the_line(){
14	+	if (analog(0)<BLACK_VALUE){
15	+	motor(0,90);
16	+	motor(1,15);
17	+	motor(2,90);
18	+	motor(3,15);
19	+	}
20	+	else{
21	+	motor(0,15);
22	+	motor(1,90);
23	+	motor(2,15);
24	+	motor(3,90);
25	+	}
26	+	}
27	+
28	+	int down_ramp()
29	+	{
30	+	printf("down ramp\n");
31	+
32	+	motor(0,100);
33	+	motor(1,100);
34	+	motor(2,100);// square up
35	+	motor(3,100);
36	+	msleep(2200);
37	+	cmpc(0);
38	+	while (stop_at_line(9000) ==0){
39	+	follow_the_line();
40	+	msleep(30);
41	+	}
42	+	return 0;
43	+	}
44	+	int up_ramp(){
45	+	printf("up ramp/n");
46	+	motor(0,100);
47	+	motor(1,100);
48	+	motor(2,100);
49	+	motor(3,100);
50	+	msleep(5500);
51	+
52	+	cmpc(0);
53	+	while (stop_at_line(6000) ==0){
54	+	follow_the_line();
55	+	msleep(30);
56	+	}
57	+	return 0;
58	+	}
59	+
60	+	void mechanum_drive_forward_tick(int distance){
61	+	cmpc(0);
62	+	int direction;
63	+	if(distance > 0){
64	+	direction = 1;
65	+	} else {direction = -1;}
66	+	motor(0,70*direction);
67	+	motor(1,70*direction);
68	+	motor(2,70*direction);
69	+	motor(3,70*direction);
70	+	int target_pos = distance;
71	+	while(abs(gmpc(0)) < abs(target_pos)){
72	+	printf("zzz...\n\n");
73	+	msleep(10);
74	+	}
75	+	ao();
76	+	}
77	+
78	+	void section_1() {
79	+	/*
80	+	* Task: Drive from upper floor start box to the lower start box while pushing poms, then drops the poms
81	+	* turns right and drives into the middle area and strafes sideways until lined up with the doors
82	+	*
83	+	* Start Position: upper start box facing ramp
84	+	* End Position: facing doors, backed against the side of the upper storage
85	+	*
86	+	*/
87	+	down_ramp();
88	+
89	+	ao();
90	+	msleep(500);
91	+
92	+	motor(0,100);
93	+	motor(1,100);
94	+	motor(2,100);
95	+	motor(3,100);
96	+	msleep(1000);
97	+
98	+	ao();
99	+	msleep(500);
100	+
101	+	motor(0,-100);
102	+	motor(1,-100);
103	+	motor(2,100);
104	+	motor(3,100);
105	+	msleep(1160);
106	+
107	+	ao();
108	+	msleep(500);
109	+
110	+	motor(0,-100);
111	+	motor(1,-100);
112	+	motor(2,-100);
113	+	motor(3,-100);
114	+	msleep(1000);
115	+
116	+	ao();
117	+	msleep(500);
118	+
119	+	// End section_1
120	+	}
121	+
122	+	void section_2(){
123	+	/*
124	+	Task: drive forward, open the doors, grab botguy, backup
125	+
126	+	Start position: back against the wall facing the doors
127	+	End position: back against the wall facing the doors
128	+	*/
129	+	printf("ROUTE#I\n\n");
130	+
131	+	cmpc(0);
132	+	while (stop_at_line(50) ==0){
133	+	motor(0,100);
134	+	motor(1,100);
135	+	motor(2,100);
136	+	motor(3,100);
137	+	msleep(30);
138	+	}
139	+	ao();
140	+	msleep(900);
141	+	//code for moving left
142	+	motor(0,70);
143	+	motor(1,-70);
144	+	motor(2,70);
145	+	motor(3,-70);
146	+	msleep(8000);
147	+	mechanum_drive_forward_tick(2000);
148	+	}
149	+
150	+	void section_3() {
151	+	/*
152	+	Task: strafe left until it's in front of the start box, backs up and squares up against the outer wall,
153	+	then turns right and backs up to square up against outer wall while facing the ramp
154	+
155	+	Start Position: back against the upper storage wall facing the doors
156	+	End Position: back against outer wall facing the ramp
157	+
158	+	*/
159	+	motor(0,-100);
160	+	motor(1,-100);
161	+	motor(2,-100);
162	+	motor(3,-100);
163	+	msleep(700);
164	+	printf("go backwards\n");
165	+	set_servo_position(3,2047);//put down arm
166	+	msleep(1000);
167	+	printf("arm down\n");
168	+	set_servo_position(0,2047);//close claw //pick up cone
169	+	msleep(1000);
170	+	printf("claw closed\n");
171	+	set_servo_position(0,900);//open claw
172	+	msleep(1000);
173	+	printf("claw open\n");
174	+	motor(0,-100);
175	+	motor(1,100);
176	+	motor(2,-100);
177	+	motor(3,100);
178	+	msleep(4600);
179	+	printf("go sideways\n");
180	+	motor(0,-100);
181	+	motor(1,-100);
182	+	motor(2,-100);
183	+	motor(3,-100);
184	+	msleep(3300);
185	+	printf("go backwards\n");
186	+	motor(0,-100);
187	+	motor(1,-100);
188	+	motor(2,100);
189	+	motor(3,100);
190	+	msleep(1300);
191	+	printf("turn\n");
192	+	motor(0,-100);
193	+	motor(1,-100);
194	+	motor(2,-100);
195	+	motor(3,-100);
196	+	msleep(1600);
197	+	printf("go backwards\n");
198	+
199	+	}
200	+
201	+	void section_4() {
202	+	/*
203	+	Task: drives up the ramp to drop botguy in the upper storage startbox
204	+
205	+	Start Position: back against outer wall facing the ramp
206	+	End Position: inside upper storage start box with botguy on the ground
207	+	*/
208	+	up_ramp();
209	+	}
210	+	int main(){
211	+
212	+	printf("Starting Section 1\n");
213	+	section_1();
214	+
215	+	printf("Starting Section 2\n");
216	+	section_2();
217	+
218	+	printf("Starting Section 3\n");
219	+	section_3();
220	+
221	+	printf("Starting Section 4\n");
222	+	}

Newer Older