94 lines
1.9 KiB
Arduino
94 lines
1.9 KiB
Arduino
#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);
|
|
}
|