uni

Thing1's amazing uni repo
Log | Files | Refs

helper.h (2368B)

      1 #include <Servo.h>
      2 
      3 #define GREEN_LED 7
      4 #define YELLOW_LED 12
      5 #define RED_LED 13
      6 
      7 #define BUTTONA 4
      8 #define BUTTONB 2
      9 
     10 #define SERVOA_PIN 6
     11 #define SERVOB_PIN 5
     12 
     13 #define LDRA A2
     14 #define LDRB A1
     15 #define LDRC A0
     16 
     17 #define SERVOA_ZERO 96
     18 #define SERVOB_ZERO 96
     19 #define ANALOG_MAX 1023
     20 
     21 #define LDRA_SWITCH (ANALOG_MAX / 2) + 100
     22 #define LDRB_SWITCH (ANALOG_MAX / 2) + 100
     23 #define LDRC_SWITCH (ANALOG_MAX / 2) - 50
     24 
     25 
     26 
     27 #define SIZE(arr) (sizeof(arr) / sizeof(arr[0]))
     28 
     29 enum DIRECTIONS {
     30 	FWD = 1,
     31 	BWD = -1,
     32 	LFT = -2,
     33 	RGT = 2
     34 };
     35 
     36 Servo servoA;
     37 Servo servoB;
     38 
     39 void 
     40 setupRobot() {
     41   Serial.begin(9600);
     42 	pinMode(GREEN_LED, OUTPUT); 
     43 	pinMode(YELLOW_LED, OUTPUT); 
     44 	pinMode(RED_LED, OUTPUT); 
     45 	pinMode(BUTTONA, INPUT); 
     46 	pinMode(BUTTONB, INPUT);
     47 	pinMode(SERVOA_PIN, OUTPUT); 
     48 	pinMode(SERVOB_PIN, OUTPUT);
     49 	pinMode(LDRA, INPUT);
     50 	pinMode(LDRB, INPUT);
     51 	pinMode(LDRC, INPUT);
     52 	servoA.attach(SERVOA_PIN); 
     53 	servoB.attach(SERVOB_PIN); 
     54 	servoA.write(SERVOA_ZERO);
     55 	servoB.write(SERVOB_ZERO);
     56 }
     57 
     58 void
     59 setLEDs(int g, int y, int r) {
     60 	digitalWrite(GREEN_LED, g);
     61 	digitalWrite(YELLOW_LED, y);
     62 	digitalWrite(RED_LED, r);
     63 }
     64 
     65 void
     66 waitButton(int pin, int state) {
     67 	while (digitalRead(pin) == state) ;
     68 }
     69 
     70 void
     71 debounce(int pin) {
     72 	waitButton(pin, LOW);
     73 	delay(20);
     74 	waitButton(pin, HIGH);	
     75 	delay(20);
     76 }
     77 
     78 void 
     79 SEprintf(const char *fmt, ...) {
     80 	static char sbuf[64] = { 0 };
     81 	va_list ap;
     82 
     83 	va_start(ap, fmt);
     84 	vsnprintf(sbuf, 64, fmt, ap);
     85 	va_end(ap);
     86 
     87 	Serial.print(sbuf);
     88 }
     89 
     90 int 
     91 readint() {
     92 	while (!Serial.available()) ; 
     93 	int i = Serial.parseInt();
     94 	SEprintf("\n");
     95 	return i;
     96 }
     97 
     98 void
     99 stepMove(int dir) {
    100 	servoA.write(SERVOA_ZERO + (40 * dir));
    101 	servoB.write(SERVOA_ZERO - (40 * dir));
    102 	delay(10);
    103 	servoA.write(SERVOA_ZERO);
    104 	servoB.write(SERVOA_ZERO);
    105 }
    106 
    107 void
    108 stepTurn(int dir) {
    109 	servoA.write(SERVOA_ZERO + (10 * dir));
    110 	servoB.write(SERVOA_ZERO - (10 * -dir));
    111 	delay(10);
    112 	servoA.write(SERVOA_ZERO);
    113 	servoB.write(SERVOA_ZERO);
    114 }
    115 
    116 void
    117 rotate(int x) {
    118 	int dir = (x < 0) ? -1 : 1;
    119 	x = (x < 0) ? -x : x;
    120 
    121 	int y = (int)(1.666666 * (float)x);
    122 
    123 	for (int i = 0; i < y; i++)
    124 		stepTurn(dir);
    125 }
    126 
    127 bool
    128 isButtonAPressed() {
    129 	return !digitalRead(BUTTONA);
    130 }
    131 
    132 bool
    133 isButtonBPressed() {
    134 	return !digitalRead(BUTTONB);
    135 }
    136 
    137 void
    138 moveFWD(void) {
    139 	stepMove(FWD);
    140 }
    141 
    142 void
    143 moveBWD(void) {
    144 	stepMove(BWD);
    145 }
    146 
    147 void
    148 stepLFT(void) {
    149 	stepTurn(LFT);
    150 }
    151 
    152 void
    153 stepRGT(void) {
    154 	stepTurn(RGT);
    155 }