uni

Thing1's amazing uni repo
Log | Files | Refs

helper.h (2562B)

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