update

2019-teensy-nrf24/robot/Hexapod_Teensy.ino

@@ -0,0 +1,93 @@
+#include "Nrf.h"
+#include <Wire.h>
+#include <MPU6050_tockn.h>
+#include "PWMServo.h"
+#include "config.h"
+
+PWMServo servos[18];
+
+MPU6050 mpu (Wire);
+
+dataStruct2 data;
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial1.begin(115200);
+  //  while (!Serial) ; // wait for arduino serial monitor
+
+  for (int i = 0; i < 18; i++)
+    servos[i].attach(pins[i], servoMin[i] + servoCallibration[i],
+                     servoMax[i] + servoCallibration[i]); // 900-2100 for high torque servos
+//  while (true)
+//  {
+    for (int i = 0; i < 18; i++)
+      servos[i].write(90 + servoOffset[i]);
+//  }
+
+
+  Serial.println("Starting mpu6050");
+
+//  //   Initialization
+//  Wire.begin();
+//  mpu.begin();
+//
+//  mpu.calcGyroOffsets(true, 300, 1000);
+//
+//  // Calibration
+//  Serial.println("=====================================");
+//  Serial.println("Starting calibration...");
+//  mpu.calcGyroOffsets();
+//  Serial.println("Calibration complete!");
+
+  Serial.println("Starting nrf");
+  NrfInit();
+}
+
+void STOP()
+{
+  for (int i = 0; i < 18; i++)
+    pinMode(pins[i], INPUT);
+  while (true)
+  {}
+}
+
+int i = 0;
+
+void loop()
+{
+  network.update();
+
+  if (false)
+  {
+    mpu.update();
+    Serial.print("AngX = ");
+    Serial.print(mpu.getAngleX());
+    Serial.print("  /  AngY = ");
+    Serial.print(mpu.getAngleY());
+    Serial.print("  /  AngZ = ");
+    Serial.println(mpu.getAngleZ());
+  }
+
+  //    Serial.print("  /  AngX = ");
+  //    Serial.print(mpu.getGyroAngleX());
+  //    Serial.print("  /  AngY = ");
+  //    Serial.print(mpu.getGyroAngleY());
+  //    Serial.print("  /  AngZ = ");
+  //    Serial.println(mpu.getGyroAngleZ());
+
+  data = NrfGetData(data, true);
+
+//  if (data.STOP)
+//    STOP();
+
+  // test
+  //  data.servoPositions[0] = data.servoPositions[0] + 0.1f;
+  //  data = NrfSendData(data, false);
+
+  if (data.remotePasstroughControl == true)
+    for (int i = 0; i < 18; i++)
+      servos[i].write(data.servoPositions[i] + servoOffset[i]);
+
+  delay(20);
+}