forked from Danielm91/osu_ork
-
Notifications
You must be signed in to change notification settings - Fork 0
/
LineFollow.c
72 lines (62 loc) · 1.93 KB
/
LineFollow.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
/*************************************************************************
*
* OrkMain.c
* ORKWare Main Code Starter File
* Library for use with 2011 OSURC Robotics Kit (ORK).
*
* Copyright (C) 2011 OSURC. Released under the GNU LGPL3.
* http://oregonstate.edu/groups/osurc/
*
* Written by Cody Hyman <hymanc@onid.orst.edu>
* Created 9/2/2011
* Updated 10/9/2011
*************************************************************************/
//#define L298 1 //Uncomment this definition to use the L298 (Upgrade Motor Driver) motor driver code
#include "OrkLib/OrkCore.h"
#include "OrkLib/uart.h"
#include <stdlib.h>
#include <stdio.h>
#define OUTER_LEFT !(PINF & (1 << 5))
#define INNER_LEFT !(PINB & (1 << 4))
#define INNER_RIGHT !(PIND & (1 << 6))
#define OUTER_RIGHT !(PIND & (1 << 4))
int main(void)
{
initializeCore(); // Initializes core ORK Functionality (motors,
setMotor(LeftMotor,100);
setMotor(RightMotor,100);
//Set PORTB Pin 1 to be an output
DDRB |= 0b00000001;
while(1)
{
if(!(OUTER_LEFT) && (INNER_LEFT) && (INNER_RIGHT) && !(OUTER_RIGHT)){
setMotor(LeftMotor,100);
setMotor(RightMotor,100);
}
else if(!(OUTER_LEFT) && !(INNER_LEFT) && (INNER_RIGHT) && !(OUTER_RIGHT)){
setMotor(LeftMotor,125);
setMotor(RightMotor,50);
}
else if(!(OUTER_LEFT) && !(INNER_LEFT) && (INNER_RIGHT) && (OUTER_RIGHT)){
setMotor(LeftMotor,125);
setMotor(RightMotor,25);
}
else if(!(OUTER_LEFT) && (INNER_LEFT) && !(INNER_RIGHT) && !(OUTER_RIGHT)){
setMotor(LeftMotor,50);
setMotor(RightMotor,125);
}
else if((OUTER_LEFT) && (INNER_LEFT) && !(INNER_RIGHT) && !(OUTER_RIGHT)){
setMotor(LeftMotor,25);
setMotor(RightMotor,125);
}
else if(!(OUTER_LEFT) && !(INNER_LEFT) && !(INNER_RIGHT) && (OUTER_RIGHT)){
setMotor(LeftMotor,125);
setMotor(RightMotor,20);
}
else if((OUTER_LEFT) && !(INNER_LEFT) && !(INNER_RIGHT) && !(OUTER_RIGHT)){
setMotor(LeftMotor,20);
setMotor(RightMotor,125);
}
}
return 0;
}