GRITSBot_firmware/basic_firmware.cpp at master · robotarium/GRITSBot_firmware · 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
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
/* Define this variable to enable the compilation
   of the right I2C interface
 */
#define ESP8266

/* Include NeoPixel library for WS2812 LEDs */
#include <Adafruit_NeoPixel.h>

/* Include basic Arduino libraries */
#include <EEPROM.h>
#include <Arduino.h>
#include <Wire.h>

/* Include ESP8266 Wifi libraries */
#include <ESP8266WiFi.h>
#include <WiFiUdp.h>

/* Include JSON parser */
#include <ArduinoJson.h>

/*include OTA Libraries */
#include <ArduinoOTA.h>
#include <ESP8266mDNS.h>

/* Include current sensor library */
//#include <Adafruit_INA219.h>

/* Include GRITSBot header files */
#include "GRITSBot_Main/GRITSBot_Main_ESP8266.h"
#include "GRITSBot_Messages/GRITSBot_Messages.h"

/* Include GRITSBot interface definitions for I2C */
#include "GRITSBot_I2CInterface/include/I2CMessage.h"
#include "GRITSBot_I2CInterface/I2CInterface.h"

/* Include GRITSBot interface definitions for WiFi and WiFi credentials*/
#include "GRITSBot_WirelessInterface/wirelessInterfaceESP8266.h"
#include "GRITSBot_WiFiConfig/wifiConfig.h"

/* Include estimator and controller libraries */
#include "GRITSBot_Main/include/controller/controllerBase.h"
#include "GRITSBot_Main/include/controller/controllerTarget.h"
#include "GRITSBot_Main/include/estimator/estimatorBase.h"

/* Instantiate Wifi UDP client */
WirelessInterfaceESP8266 wifi;

/* Instantiate I2C client */
I2CInterface i2c;

/* Instantiate currenet sensor */
Adafruit_INA219 ina219;

/* Instantiate controller and estimator */
ControllerTarget controller;
EstimatorBase estimator;

/* Instantiate WS2812 RGB LED strip */
Adafruit_NeoPixel strip = Adafruit_NeoPixel(2, LED_PIN, NEO_GRB + NEO_KHZ800);

/* Instantiate main board */
GRITSBotMain mainboard(&wifi, &i2c, &ina219, &strip, &controller, &estimator);

/* Set velocities for forward, backward, rotate cw, rotate ccw */
float vSet[] = {0.1, 0.1,  0.0,   0.0};
float wSet[] = {0.0,  0.0, 180.0 * M_PI/180.0, -180.0 * M_PI/ 180.0};
String dir[] = {"forward", "backward", "CCW", "CW"};
unsigned long lastTransition = micros();
int mode;
bool enableRandomWalk = 0;
/* **********************
 *     SETUP FUNCTION
 * **********************/
void setup() {
  Serial.begin(115200);
  Serial.println("Main board started");

  /* Initialize boards */
  mainboard.initialize();

  /* REMOVE: Disable motor voltage for testing */
  mainboard.enableMotorVoltage();

  /* Initializing OTA */
  ArduinoOTA.begin();
  Serial.println("Ready for OTA firmware updates");
  Serial.println("Main board initialized");
  Serial.print("Version Firmware Main : "); Serial.println(mainboard.getMainBoardFirmwareVersion());
  Serial.print("Version Hardware Main : "); Serial.println(mainboard.getMainBoardHardwareVersion());
  Serial.print("Version Firmware Motor: "); Serial.println(mainboard.getMotorBoardFirmwareVersion());
  Serial.print("Version Hardware Motor: "); Serial.println(mainboard.getMotorBoardHardwareVersion());

  /* Rainbow RGB LED animation */
  mainboard.rainbow(10);
  mainboard.disableLedsRGB();
}

/* ********************
 *     MAIN LOOP
 * ********************/
void loop() {
  mainboard.enableMotorVoltage();
  /* Update wireless function takes care of all message processing
   * and sends out heartbeat message every second
   */
  yield();
  mainboard.updateWireless();

  if (enableRandomWalk) {
  /* Random transitions every 2 seconds */
  if( (lastTransition + 2 * 1E6) < micros() ) {
    lastTransition = micros();
    mode = random(0, 4);
    mainboard.setVelocities(vSet[mode], wSet[mode]);
    Serial.print("Transition to mode: "); Serial.println(dir[mode]);
    }
    mainboard.setVelocities(vSet[mode], wSet[mode]);
    delay(20);
  }
  else {
    /* Update controller */
    yield();
    mainboard.updateController();
 }

  /* Update measurement and data collection */
  yield();
  mainboard.updateMeasurements();

  /*OTA handling*/
  ArduinoOTA.handle();
}