Use new motor driver and added turning signals

.gitignore

@@ -3,4 +3,6 @@ OldVersions/
 *.gcode
 *.gco
 *.iam
-lockfile.lck
+lockfile.lck
+.pioenvs
+.piolibdeps

BalancingRobotFullSize.h

@@ -18,8 +18,6 @@
 #ifndef _balancingrobotfullsize_h_
 #define _balancingrobotfullsize_h_
 
-#include <stdint.h> // Needed for uint8_t, uint16_t etc.
-
 extern double turningValue;
 extern uint32_t kalmanTimer;
 extern uint16_t batteryLevel;

BalancingRobotFullSize.ino

@@ -71,6 +71,10 @@ void setup() {
 
   zeroTurning = getTurning(); // Calibrate turning
 
+  /* Set turning pins used for turning signals */
+  turningLeft::SetDirWrite();
+  turningRight::SetDirWrite();
+
   /* Beep to indicate that it is now ready */
   buzzer::Set();
   delay(100);
@@ -82,25 +86,21 @@ void setup() {
 }
 
 void loop () {
-#if 0
-  if (leftF1::IsSet() || leftF2::IsSet() || rightF1::IsSet() || rightF2::IsSet()) { // Check the diagnostic pins
-    buzzer::Set();
-    Serial.print(F("Diagnostic error: "));
-    Serial.print(leftF1::IsSet());
-    Serial.write('\t');
-    Serial.print(leftF2::IsSet());
-    Serial.write('\t');
-    Serial.print(rightF1::IsSet());
-    Serial.write('\t');
-    Serial.println(rightF2::IsSet());
-  }
-#endif
-
   if (dataReady::IsSet()) { // Check is new data is ready
     updateAngle();
 
     turningValue = getTurning() - zeroTurning; // Update turning value
 
+    /* Set turning signals */
+    static bool turning; // Used for a deadband in the middle, so the LEDs do not flicker
+    if (abs(turningValue) > 5)
+      turning = true;
+    else if (abs(turningValue) < 1)
+      turning = false;
+
+    turningLeft::Set(turning && turningValue > 1);
+    turningRight::Set(turning && turningValue < -1);
+
     /* Drive motors */
     uint32_t timer = micros();
     // If the robot is laying down, it has to be put in a vertical position before it starts balancing

EEPROM.cpp

@@ -15,6 +15,8 @@
  e-mail   :  [email protected]
 */
 
+#include <stdint.h> // Needed for uint8_t, uint16_t etc.
+
 #include "EEPROM.h"
 #include "EEPROMAnything.h"
 #include "IMU.h"

EEPROM.h

@@ -18,8 +18,6 @@
 #ifndef _eeprom_h_
 #define _eeprom_h_
 
-#include <stdint.h> // Needed for uint8_t, uint16_t etc.
-
 // This struct will store all the configuration values
 typedef struct {
   double Kp, Ki, Kd; // PID variables

I2C.h

@@ -18,8 +18,6 @@
 #ifndef _i2c_h_
 #define _i2c_h_
 
-#include <stdint.h> // Needed for uint8_t, uint16_t etc.
-
 void initI2c();
 uint8_t i2cWrite(uint8_t address, uint8_t registerAddress, uint8_t data, bool sendStop = true);
 uint8_t i2cWrite(uint8_t address, uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop = true);

Makefile

@@ -12,6 +12,6 @@ BOARD = pro
 BOARD_SUB = pro.menu.cpu.16MHzatmega328
 MON_SPEED = 57600
 
-INC_DIRS = $(CURDIR)/KalmanFilter
+INTERNAL_LIB_DIRS = $(CURDIR)/KalmanFilter
 
 include Arduino_Makefile_master/_Makefile.master

Motor.cpp

@@ -25,8 +25,8 @@ void setPWM(Command motor, uint16_t dutyCycle);
 
 void initMotors() {
   /* Set the motor driver diagnostic pins to inputs */
-  leftF1::SetDirRead();
-  leftF2::SetDirRead();
+  /*leftF1::SetDirRead();
+  leftF2::SetDirRead();*/
   rightF1::SetDirRead();
   rightF2::SetDirRead();
 

PID.cpp

@@ -40,7 +40,7 @@ void updatePID(double restAngle, double offset, double turning, double dt) {
   lastError = error;
   PIDValue = pTerm + iTerm + dTerm;
 
-  currentSpeed = (currentSpeed + PIDValue * 0.004) * 0.999; // TODO: Explain this
+  currentSpeed = (currentSpeed + PIDValue * 0.005) * 0.999; // TODO: Explain this
   //currentSpeed = constrain(currentSpeed, -50, 50);
 
   //Serial.print(PIDValue); Serial.write('\t'); Serial.print(currentSpeed); Serial.write('\t');

Pins.h

@@ -42,8 +42,8 @@
 #define rightPWMReg OCR1A
 
 /* Pins connected to the motor drivers diagnostic pins */
-#define leftF1 P5
-#define leftF2 P6
+/*#define leftF1 P5
+#define leftF2 P6*/
 
 #define rightF1 P3
 #define rightF2 P4
@@ -52,6 +52,10 @@
 #define STEER_PIN A0
 #define VBAT_PIN  A1
 #define CS1_PIN   A6
-#define CS2_PIN   A7
+//#define CS2_PIN   A7
+
+/* Turn signals */
+#define turningLeft P5
+#define turningRight P6
 
 #endif

Protocol.cpp

@@ -57,10 +57,10 @@ struct info_t {
   uint32_t runTime;
 } __attribute__((packed)) info;
 
-struct imu_t {
-  uint16_t acc;
-  uint16_t gyro;
-  uint16_t kalman;
+struct imu_t { // Note that this can be negative as well
+  int16_t acc;
+  int16_t gyro;
+  int16_t kalman;
 } __attribute__((packed)) imu;
 
 #define SET_PID     0
@@ -316,7 +316,7 @@ void parseSerialData() {
     info.speed = constrain(abs(PIDValue), 0, 100.0) * 100.0;
     if (deadmanButton::IsSet()) {
       double CS = ((double)analogRead(CS1_PIN) / 204.6 - 2.5) / 0.066 * 100.0; // 66mV/A and then multiply by 100.0
-      CS -= ((double)analogRead(CS2_PIN) / 204.6 - 2.5) / 0.066 * 100.0; // The motors turn opposite, so we need to subtract the value
+      //CS -= ((double)analogRead(CS2_PIN) / 204.6 - 2.5) / 0.066 * 100.0; // The motors turn opposite, so we need to subtract the value
       info.current = CS;
     } else
       info.current = 0; // When the reset button is held low on the motor drivers, the current sensor will give out an incorrect value

TurnSignal/TurnSignal.ino

@@ -0,0 +1,84 @@
+/* Copyright (C) 2015 Kristian Sloth Lauszus. All rights reserved.
+
+ This software may be distributed and modified under the terms of the GNU
+ General Public License version 2 (GPL2) as published by the Free Software
+ Foundation and appearing in the file GPL2.TXT included in the packaging of
+ this file. Please note that GPL2 Section 2[b] requires that all works based
+ on this software must also be made publicly available under the terms of
+ the GPL2 ("Copyleft").
+
+ Contact information
+ -------------------
+
+ Kristian Sloth Lauszus
+ Web      :  http://www.lauszus.com
+ e-mail   :  [email protected]
+*/
+
+#include <Adafruit_NeoPixel.h>
+
+#define NUM_PIXELS     48
+#define TURN_PIXELS    5
+
+#define GREEN          pixels.Color(0, 255, 0)
+#define RED            pixels.Color(255, 0, 0)
+#define ORANGE         pixels.Color(255, 135, 0)
+
+static const uint8_t turningLeftPin = A2, turningRightPin = A1, pixelsPin = A3;
+
+Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUM_PIXELS, pixelsPin, NEO_GRB + NEO_KHZ800);
+
+bool turnSignalState, turning;
+uint32_t timer; // Left and right turn signals can't be on at the same time, so we can just use the same timer for both
+
+void setup() {
+  pinMode(turningLeftPin, INPUT);
+  pinMode(turningRightPin, INPUT);
+
+  pixels.begin();
+}
+
+void loop() {
+  setMiddleAndBackOn(); // Set all lights on. This may get overriden by left and right turn signals
+
+  bool turningLeft = digitalRead(turningLeftPin);
+  bool turningRight = digitalRead(turningRightPin);
+  uint32_t now = millis();
+  if (turningLeft || turningRight) {
+    turning = true;
+    if ((int32_t)(now - timer) > 250) { // Blink every 250 ms
+      timer = now;
+      turnSignalState = !turnSignalState;
+    }
+    if (turningLeft)
+      setLeftTurn(turnSignalState);
+    else if (turningRight)
+      setRightTurn(turnSignalState);
+  } else if (turning) {
+    turning = false;
+    delay(250 - min((now > timer ? now - timer : 0), 250)); // Make sure now is larger and the difference is no more than 250
+    timer = now; // Reset timer
+    turnSignalState = false; // Set LED state back to off
+  }
+
+  pixels.show();
+}
+
+void setMiddleAndBackOn() {
+  for (uint8_t i = 0; i < NUM_PIXELS / 2; i++)
+    pixels.setPixelColor(i, RED); // Back LEDs
+  for (uint8_t i = NUM_PIXELS / 2; i < NUM_PIXELS; i++)
+    pixels.setPixelColor(i, GREEN); // Front LEDs
+}
+
+void setLeftTurn(bool enable) { // The left turn signals are in the ends of the LED strip
+  for (uint8_t i = 0; i < TURN_PIXELS; i++)
+    pixels.setPixelColor(i, enable ? ORANGE : 0);
+  for (uint8_t i = NUM_PIXELS - TURN_PIXELS; i < NUM_PIXELS; i++)
+    pixels.setPixelColor(i, enable ? ORANGE : 0);
+}
+
+void setRightTurn(bool enable) { // The right turn signals are in the center of the LED strip
+  for (uint8_t i = NUM_PIXELS / 2 - TURN_PIXELS; i < NUM_PIXELS / 2 + TURN_PIXELS; i++)
+    pixels.setPixelColor(i, enable ? ORANGE : 0);
+}

TurnSignal/platformio.ini

@@ -0,0 +1,13 @@
+[platformio]
+src_dir = ./
+
+[env:pro_mini]
+platform = atmelavr
+framework = arduino
+board = pro16MHzatmega328
+lib_deps = Adafruit NeoPixel
+
+; Basic commands:
+; platformio run
+; platformio run -t upload
+; platformio run -t clean

platformio.ini

@@ -0,0 +1,14 @@
+[platformio]
+src_dir = ./
+
+[env:pro_mini]
+platform = atmelavr
+framework = arduino
+board = pro16MHzatmega328
+lib_deps = Kalman Filter Library
+src_filter = +<*.ino> +<*.cpp>
+
+; Basic commands:
+; platformio run
+; platformio run -t upload
+; platformio run -t clean