src/DroneClaw/DroneClaw.ino

@@ -7,7 +7,7 @@
 #include "PID.hpp"
 #include "MPU.hpp"
 
-#define DEBUG // Will enable debug code throught the program
+//#define DEBUG // Will enable debug code throught the program
 #define DEBUG_MOTOR 1100
 
 #define FL_ESC 0

src/DroneClaw/PID.hpp

@@ -14,17 +14,17 @@
 #define FREQUENCY_RAD FREQUENCY * DEG_TO_RAD
 #define MAX_TILT 400
 
-#define P_ROLL 1.3
-#define I_ROLL 0.04
-#define D_ROLL 18.0
+#define P_ROLL 1
+#define I_ROLL 0
+#define D_ROLL 0
 
-#define P_PITCH 1.3
-#define I_PITCH 0.04
-#define D_PITCH 18.0
+#define P_PITCH 1
+#define I_PITCH 0
+#define D_PITCH 1
 
-#define P_YAW 4.0
-#define I_YAW 0.02
-#define D_YAW 0.0
+#define P_YAW 1
+#define I_YAW 0
+#define D_YAW 0
 
 class PID {
   private:

src/TaranisModule/TaranisModule.ino

@@ -1,35 +1,38 @@
 /*
     DroneClaw copyright 2016
 */
-#include <CurieEEPROM.h>
+#include <EEPROM.h>
 #include <EventLoop.h>
+#include <SoftwareSerial.h>
 
 #define PIN 2
 #define CHANNELS 4
 #define VERSION 0
 
+SoftwareSerial stream(10, 11);
 EventLoop scheduler;
 
 struct {
   // Channel A - throttle
-  int a_min = 187;
-  int a_mid = 757;
-  int a_max = 1211;
+  unsigned long a_min = 187;
+  unsigned long a_mid = 757;
+  unsigned long a_max = 1211;
   // Channel B - pitch
-  int b_min = 187;
-  int b_mid = 699;
-  int b_max = 1211;
+  unsigned long b_min = 187;
+  unsigned long b_mid = 699;
+  unsigned long b_max = 1211;
   // Channel C - roll
-  int c_min = 187;
-  int c_mid = 698;
-  int c_max = 1211;
+  unsigned long c_min = 187;
+  unsigned long c_mid = 698;
+  unsigned long c_max = 1211;
   // Channel D - yaw
-  int d_min = 189;
-  int d_mid = 702;
-  int d_max = 1211;
+  unsigned long d_min = 189;
+  unsigned long d_mid = 702;
+  unsigned long d_max = 1211;
 } config_eeprom;
 
 void setup() {
+  stream.begin(115200);
   Serial.begin(9600);
   pinMode(PIN, INPUT);
   //EEPROM.get(sizeof(byte), config_eeprom);
@@ -38,7 +41,7 @@ void setup() {
     while (!Serial);
     Serial.println("Get ready...");
     for (int i = 0 ; i < 1000 ; i++) {
-      int* tmp = read_channels(); // normlize the values
+      unsigned long* tmp = read_channels(); // normlize the values
       delete tmp;
     }
     delay_message(10);
@@ -49,19 +52,47 @@ void setup() {
     dump_eeprom();
     EEPROM.write(0, VERSION);
   }
+  // Connect the bluetooth to the drone
+  stream.print("$$$");
+  delay(1000);
+  stream.print("C,201602225787\r");
+  delay(5000);
+  stream.println(0);
+  delay(1000);
+  stream.println(1);
+  // send packets
+  scheduler.repeat(read_ppm, 50, MILLIS); // ppm frame
+  scheduler.repeat(ping_packet, 750, MILLIS);
+  scheduler.repeat(data_packet, 500, MILLIS);
+}
+
+/** The ping packet */
+void ping_packet() {
+  stream.println(0);
+}
+
+void read_ppm() {
+  unsigned long* tmp = read_channels();
+  delete tmp;
+}
+
+/** The data packet */
+void data_packet() {
+  unsigned long* channels = mapped_channels();
+  stream.print(3);
+  stream.print(" ");
+  stream.print(channels[0]);
+  stream.print(" ");
+  stream.print(channels[1]);
+  stream.print(" ");
+  stream.print(channels[2]);
+  stream.print(" ");
+  stream.println(channels[3]);
+  delete channels;
 }
 
 void loop() {
   scheduler.process();
-  int* channels = mapped_channels();
-  Serial.print(channels[0]);
-  Serial.print(",");
-  Serial.print(channels[1]);
-  Serial.print(",");
-  Serial.print(channels[2]);
-  Serial.print(",");
-  Serial.println(channels[3]);
-  delete channels;
 }
 
 /** Print current eeprom values to screen */
@@ -88,9 +119,9 @@ void dump_eeprom() {
 void calibrate_eeprom() {
   Serial.println("Center all sticks, with in 5 seconds");
   delay_message(5);
-  int* mids = read_channels();
+  unsigned long* mids = read_channels();
   Serial.println("Move all stick in round circles, for 10 seconds");
-  int* min_max = calibrate_sticks(10);
+  unsigned long* min_max = calibrate_sticks(10);
   Serial.println("Done");
   config_eeprom.a_mid = mids[0];
   config_eeprom.b_mid = mids[1];
@@ -119,40 +150,41 @@ void delay_message(int delta) {
 }
 
 /** Remap the channels to match what we need */
-int* mapped_channels() {
-  int* channels = read_channels();
-  channels[0] = map(channels[0], config_eeprom.a_min, config_eeprom.a_max, 1000, 1800);
+unsigned long* mapped_channels() {
+  unsigned long* channels = read_channels();
+  channels[0] = map(channels[0], config_eeprom.a_min, config_eeprom.a_max, 950, 1800);
   channels[1] = map(channels[1], config_eeprom.b_min, config_eeprom.b_max, -180, 180);
   channels[2] = map(channels[2], config_eeprom.c_min, config_eeprom.c_max, -180, 180);
   channels[3] = map(channels[3], config_eeprom.d_min, config_eeprom.d_max, -180, 180);
+  //channels[4] = map(channels[4], config_eeprom.d_min, config_eeprom.d_max, 45, 120);
   return channels;
 }
 
 /** Read the channels and return the array */
-int* read_channels() {
-  static int last_channel[CHANNELS] = {};
-  int* channels = new int[CHANNELS];
-  for (int i = 0 ; i < CHANNELS ; i++) {
+unsigned long* read_channels() {
+  static unsigned long last_channel[CHANNELS] = {};
+  unsigned long* channels = new unsigned long[CHANNELS];
+  for (unsigned int i = 0 ; i < CHANNELS ; i++) {
     last_channel[i] = channels[i] = 0.2 * last_channel[i] +  0.8 * pulseIn(PIN, HIGH);
   }
-  return channels;
+  return channels ;
 }
 
 /** Run the calibration and return the values min and max for each channel*/
-int* calibrate_sticks(int delta) {
+unsigned long* calibrate_sticks(int delta) {
   unsigned long stop_time = millis() + delta * 1000; // convert to millis from seconds
   int mins[CHANNELS] = {}, maxs[CHANNELS] = {};
-  int* channels = read_channels();
+  unsigned long* channels = read_channels();
   // init the channels with something
-  for (int i = 0 ; i < CHANNELS ; i++) {
+  for (unsigned int i = 0 ; i < CHANNELS ; i++) {
     mins[i] = channels[i];
     maxs[i] = channels[i];
   }
   // find the mins, mids, and maxs
   do {
     Serial.println(".");
-    int* channels = read_channels();
-    for (int i = 0 ; i < CHANNELS ; i++) {
+    unsigned long* channels = read_channels();
+    for (unsigned int i = 0 ; i < CHANNELS ; i++) {
       if (channels[i] < mins[i]) {
         mins[i] = channels[i];
       }
@@ -162,9 +194,9 @@ int* calibrate_sticks(int delta) {
     }
     delete channels;
   } while (millis() < stop_time);
-  int* values = new int[CHANNELS * 2];
-  int k = 0;
-  for (int i = 0; i < CHANNELS; i++) {
+  unsigned long* values = new unsigned long[CHANNELS * 2];
+  unsigned int k = 0;
+  for (unsigned int i = 0; i < CHANNELS; i++) {
     values[i++] = mins[k];
     values[i] = maxs[k++];
   }