search-rescue-robot/Control_program.cpp at main · limzerui/search-rescue-robot · GitHub

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
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
#include <errno.h>
#include <ncurses.h>
#include <stdio.h>
#include <string.h>
#include <wiringPi.h>
#include <wiringSerial.h>

int main(int argc, char *argv[]) {
  int ch = 2;
  initscr();
  cbreak();
  noecho();

  int fd;
  if ((fd = serialOpen("/dev/ttyS0", 115200)) < 0) {
    fprintf(stderr, "Unable to open serial device: %s\n", strerror(errno));
    return 1;
  }

  if (wiringPiSetup() == -1) {
    fprintf(stdout, "Unable to start wiringPi: %s\n", strerror(errno));
    return 1;
  }

  timeout(100);

  while (true) {
    char c = getch();

    switch (c) {
    // Coarse movement
    case 'w':
      ch = 1;
      break; // forward (100ms)
    case 'a':
      ch = 3;
      break; // left (100ms)
    case 's':
      ch = 5;
      break; // back (100ms)
    case 'd':
      ch = 4;
      break; // right (100ms)

    // Claw and medpack
    case 'q':
      ch = 6;
      break; // open claw
    case 'e':
      ch = 7;
      break; // close claw
    case 'z':
      ch = 0;
      break; // hold medpack
    case 'c':
      ch = 8;
      break; // deposit medpack

    // Fine movement
    case 'j':
      ch = 9;
      break; // left (25ms)
    case 'l':
      ch = 10;
      break; // right (25ms)
    case 'i':
      ch = 11;
      break; // forward (25ms)
    case 'k':
      ch = 12;
      break; // back (25ms)

    case '=': // quit
      endwin();
      return 0;

    default:
      ch = 2; // stop
      break;
    }

    serialPutchar(fd, (uint8_t)ch);

    while (serialDataAvail(fd)) {
      char received = serialGetchar(fd);
      if (received == 0)
        printw("Red  \r");
      else if (received == 1)
        printw("Green\r");
      else
        printw("Black\r");
    }

    refresh();
  }

  endwin();
  return 0;
}