diff --git a/Joysticks.h b/Joysticks.h new file mode 100644 index 0000000..eff45ab --- /dev/null +++ b/Joysticks.h @@ -0,0 +1,125 @@ +#include + +// start reading from the first byte (address 0) of the EEPROM +int address = 0; +byte value; + +// Joystick vars +const int analogInPin = A0; + +int sensorValue = 0; +int outputValue = 0; + +//volatile int center[6] = {0, 0, 0, 0, 0, 0}; +volatile int minvalue[6] = {1000, 1000, 1000, 1000, 1000, 1000}; +volatile int maxvalue[6] = {0, 0, 0, 0, 0, 0}; + +void JoystickInit() +{ + // Initialize joysticks first + value = EEPROM.read(address); + + if (value != 0) + { + address++; + for (int i = 0; i < 6; i++) + { + address += sizeof(int); + EEPROM.get(address, minvalue[i]); + address += sizeof(int); + EEPROM.get(address, maxvalue[i]); + // Serial.println(minvalue[i]); + // Serial.println(maxvalue[i]); + } + } + else + { + // if eeprom empty + delay(500); + + // for (int i = 0; i < 6; i++) + // { + // center[i] = analogRead(analogInPin + i); + // } + + int time = 10000; + while (millis() < time) + { + for (int i = 0; i < 6; i++) + { + sensorValue = analogRead(analogInPin + i); + if (sensorValue > maxvalue[i]) { + maxvalue[i] = sensorValue; + } + if (sensorValue < minvalue[i]) { + minvalue[i] = sensorValue; + } + } + } + + // for (int i = 0; i < 6; i++) + // { + // minvalue[i] = center[i] - minvalue[i]; + // maxvalue[i] = (maxvalue[i] - center[i]); + // } + + // write callibration to eeprom + address = 0; + EEPROM.write(address, 1); + + address++; + for (int i = 0; i < 6; i++) + { + address += sizeof(int); + EEPROM.put(address, minvalue[i]); + address += sizeof(int); + EEPROM.put(address, maxvalue[i]); + // Serial.println(minvalue[i]); + // Serial.println(maxvalue[i]); + } + } +} + +struct joyStickdataStruct { + float value[6]; +} myJoyStickdataStruct; + +joyStickdataStruct JoystickGet(bool debug = false) +{ + for (int i = 0; i < 6; i++) + { + sensorValue = analogRead(analogInPin + i); + + if (sensorValue > maxvalue[i]) + maxvalue[i] = sensorValue; + if (sensorValue < minvalue[i]) + minvalue[i] = sensorValue; + + outputValue = map(sensorValue, minvalue[i], maxvalue[i], 0, 255); + outputValue = sensorValue > maxvalue[i] ? 255 : sensorValue < minvalue[i] ? 0 : outputValue; + // if (sensorValue <= center[i]) + // { + // outputValue = map(sensorValue, minvalue[i], center[i], 0, 127); + // } + // else + // { + // outputValue = map(sensorValue, center[i], maxvalue[i], 128, 255); + // } + + myJoyStickdataStruct.value[i] = outputValue; + + if (debug) + { + Serial.print(i); + Serial.print("_"); + Serial.print(outputValue); + if (i != 5) + Serial.print(","); + // Serial.print(" "); + } + } + if (debug) + Serial.println(); + + return myJoyStickdataStruct; +} diff --git a/Nrf.h b/Nrf.h new file mode 100644 index 0000000..93c4a56 --- /dev/null +++ b/Nrf.h @@ -0,0 +1,65 @@ +#include +#include "RF24.h" +#include "TransferDataStructure.h" + +// Nrf24 settings +byte addresses[][6] = {"1Node", "2Node"}; +RF24 radio(9, 10); // CE, CSN + +void NrfInit() +{ + radio.begin(); + + // Set the PA Level low to prevent power supply related issues since this is a + // getting_started sketch, and the likelihood of close proximity of the devices. RF24_PA_MAX is default. + radio.setPALevel(RF24_PA_LOW); + + radio.openWritingPipe(addresses[0]); + radio.openReadingPipe(1, addresses[1]); + + // Possibly init data object here + + radio.startListening(); +} + +dataStruct2 NrfSendData(dataStruct2 data, bool debug = false) +{ + radio.stopListening(); + // First, stop listening so we can talk. + + if (debug) + Serial.println(F("Now sending")); + + if (!radio.write( &data, sizeof(data) )) { + if (debug) + Serial.println(F("failed")); + return data; + } + + radio.startListening(); + // Now, continue listening + + unsigned long started_waiting_at = micros(); + // Set up a timeout period, get the current microseconds + boolean timeout = false; + // Set up a variable to indicate if a response was received or not + + while ( ! radio.available() ) { + if (micros() - started_waiting_at > 200000 ) { + // If waited longer than 200ms, indicate timeout and exit while loop + timeout = true; + break; + } + } + + if ( timeout ) { + if (debug) + Serial.println(F("Failed, response timed out.")); + return data; + } else { + // Grab the response, compare, and send to debugging spew + radio.read( &data, sizeof(data) ); + + return data; + } +} diff --git a/SerialPasstrough.h b/SerialPasstrough.h new file mode 100644 index 0000000..3bdc98f --- /dev/null +++ b/SerialPasstrough.h @@ -0,0 +1,46 @@ +#include "TransferDataStructure.h" + +dataStruct2 GetSerialData(dataStruct2 dataIn, bool debug = false) +{ + if (Serial.available()) + { + String data = Serial.readStringUntil('\r'); + int maxIndex = data.length() - 1; + int index = 0; + int next_index; + String data_word; + String channel; + String angle; + do { + next_index = data.indexOf(',', index); + + data_word = data.substring(index, next_index); + channel = data_word.substring(0, data_word.indexOf('_')); + angle = data_word.substring(data_word.indexOf('_') + 1); + + if (isDigit(angle[0])) // prevent error overwriting first entry. +// dataIn.servoPositions[channel.toInt()] = angle.toFloat(); + + index = next_index + 1; + } while ((next_index != -1) && (next_index < maxIndex)); + +// dataIn.remotePasstroughControl = true; + + if (debug) + { + Serial.println("Enabled Servo Passtrough mode!"); + + Serial.println("Output line: "); + for (int i = 0; i < 18; i++) + { + Serial.print(i); + Serial.print(", "); +// Serial.print(dataIn.servoPositions[i]); + Serial.println(""); + } + Serial.println(""); + } + } + + return dataIn; +} diff --git a/TransferDataStructure.h b/TransferDataStructure.h new file mode 100644 index 0000000..6e6d782 --- /dev/null +++ b/TransferDataStructure.h @@ -0,0 +1,28 @@ +#ifndef dataStruct +#define dataStruct + +struct __attribute__((__packed__)) dataStruct2 +{ + float walkSpeed = 10; //step size + float walkSpeed2 = 0; //gate speed + float walkRotation = 0 ; + float walkDirection = 0; + + float tranform[3] = {0, 0, 0}; + float tranform2[3] = {0, 0, 0}; + float rotation[3] = {0, 0, 0}; + + bool remotePasstroughControl = 0; + uint16_t servoPositions[18]; +} dataStruct; + + +//template struct print_ct { +// print_ct() { +// static_assert(sizeof...(T) == -1, ""); +// } +//}; +// +//print_ct< sizeof(dataStruct2), alignof(dataStruct2) > v; + +#endif diff --git a/remote.ino b/remote.ino index eb53ab8..31d73a1 100644 --- a/remote.ino +++ b/remote.ino @@ -1,114 +1,60 @@ -#include +#include "Joysticks.h" +#include "Nrf.h" +#include "SerialPasstrough.h" +#include -// start reading from the first byte (address 0) of the EEPROM -int address = 0; -byte value; +MPU6050 mpu (Wire); -const int analogInPin = A0; - -int sensorValue = 0; -int outputValue = 0; - -//volatile int center[6] = {0, 0, 0, 0, 0, 0}; -volatile int minvalue[6] = {1000, 1000, 1000, 1000, 1000, 1000}; -volatile int maxvalue[6] = {0, 0, 0, 0, 0, 0}; +dataStruct2 data; +joyStickdataStruct joystickData; void setup() { Serial.begin(115200); - value = EEPROM.read(address); + // Initialization + mpu.Initialize(); - if (value != 0) - { - address++; - for (int i = 0; i < 6; i++) - { - address += sizeof(int); - EEPROM.get(address, minvalue[i]); - address += sizeof(int); - EEPROM.get(address, maxvalue[i]); - // Serial.println(minvalue[i]); - // Serial.println(maxvalue[i]); - } - } - else - { - // if eeprom empty - delay(500); + // // Calibration + // Serial.println("====================================="); + // Serial.println("Starting calibration..."); + // mpu.Calibrate(); + // Serial.println("Calibration complete!"); + // Serial.println("Offsets:"); + // Serial.print("AccX Offset = "); + // Serial.println(mpu.GetAccXOffset()); + // Serial.print("AccY Offset = "); + // Serial.println(mpu.GetAccYOffset()); + // Serial.print("AccZ Offset = "); + // Serial.println(mpu.GetAccZOffset()); + // Serial.print("GyroX Offset = "); + // Serial.println(mpu.GetGyroXOffset()); + // Serial.print("GyroY Offset = "); + // Serial.println(mpu.GetGyroYOffset()); + // Serial.print("GyroZ Offset = "); + // Serial.println(mpu.GetGyroZOffset()); - // for (int i = 0; i < 6; i++) - // { - // center[i] = analogRead(analogInPin + i); - // } + JoystickInit(); - int time = 10000; - while (millis() < time) - { - for (int i = 0; i < 6; i++) - { - sensorValue = analogRead(analogInPin + i); - if (sensorValue > maxvalue[i]) { - maxvalue[i] = sensorValue; - } - if (sensorValue < minvalue[i]) { - minvalue[i] = sensorValue; - } - } - } - - // for (int i = 0; i < 6; i++) - // { - // minvalue[i] = center[i] - minvalue[i]; - // maxvalue[i] = (maxvalue[i] - center[i]); - // } - - // write callibration to eeprom - address = 0; - EEPROM.write(address, 1); - - address++; - for (int i = 0; i < 6; i++) - { - address += sizeof(int); - EEPROM.put(address, minvalue[i]); - address += sizeof(int); - EEPROM.put(address, maxvalue[i]); - // Serial.println(minvalue[i]); - // Serial.println(maxvalue[i]); - } - } + NrfInit(); } void loop() { - for (int i = 0; i < 6; i++) - { - sensorValue = analogRead(analogInPin + i); + // mpu.Execute(); + // Serial.print("AngX = "); + // Serial.print(mpu.GetAngX()); + // Serial.print(" / AngY = "); + // Serial.print(mpu.GetAngY()); + // Serial.print(" / AngZ = "); + // Serial.println(mpu.GetAngZ()); + joystickData = JoystickGet(true); - if (sensorValue > maxvalue[i]) - maxvalue[i] = sensorValue; - if (sensorValue < minvalue[i]) - minvalue[i] = sensorValue; + data = GetSerialData(data, false); - outputValue = map(sensorValue, minvalue[i], maxvalue[i], 0, 255); - outputValue = sensorValue > maxvalue[i] ? 255 : sensorValue < minvalue[i] ? 0 : outputValue; - // if (sensorValue <= center[i]) - // { - // outputValue = map(sensorValue, minvalue[i], center[i], 0, 127); - // } - // else - // { - // outputValue = map(sensorValue, center[i], maxvalue[i], 128, 255); - // } + // data.servoPositions[0] = joystickData.value[0]; + data.remotePasstroughControl = data.remotePasstroughControl + 1; - Serial.print(i); - Serial.print("_"); - Serial.print(outputValue); - if (i != 5) - Serial.print(","); - // Serial.print(" "); - } - Serial.println(); + data = NrfSendData(data, true); delay(20); }