motorTest

////////////////////////////////////////////////////////
//Define libraries
////////////////////////////////////////////////////////
//The libary section of the code does not need to be altered

// The Intel Curie libraries are required for reading sensor values and
// communicating with Bluetooth
#include <CurieBLE.h>
#include <CurieIMU.h>

// Servo library is required for sending high frequency PWM
// signals to the ESCs.
#include <Servo.h>

// BLE library is required to send data to the USB BLE dongle
#include <BLESerial.h>

////////////////////////////////////////////////////////
//Define variables
////////////////////////////////////////////////////////

//System variables
int serialPrintCounter = 0;
int bluetoothPrintCounter = 0;
char BLEString[50];
long sampleTime;
long sampleTimeOld;
float sampleTimeMillis;

//Motor control variables
int addressESCLeftFront = 3;  //Svart
int addressESCRightFront = 5; //SvartR
int addressESCLeftBack = 6;   //RödR
int addressESCRightBack = 9;  //Röd

int controlSignal;

Servo ESCRightFront;
Servo ESCLeftFront;
Servo ESCRightBack;
Servo ESCLeftBack;

////////////////////////////////////////////////////////
//Functions for handling interrupts
////////////////////////////////////////////////////////

void setup()
{
  //Code in setup() function will only execute once, when power is supplied to
  //the CPU, when restarted or when new firmware is loaded

  ////////////////////////////////////////////////////////
  //System setup
  ////////////////////////////////////////////////////////

  Serial.begin(9600); // Start serial communication (with computer) at 57600 bps
  while (!Serial);

  sampleTimeOld = micros();

  //BLESerial.setName("Bluno101"); // Start bluetooth communication
  //BLESerial.begin();
  //while(!BLESerial);

  ////////////////////////////////////////////////////////
  //Initiate the ESCs
  ////////////////////////////////////////////////////////

  //Declare the address (pin) of the ESCs
  pinMode(addressESCLeftFront, OUTPUT);
  pinMode(addressESCRightFront, OUTPUT);
  pinMode(addressESCLeftBack, OUTPUT);
  pinMode(addressESCRightBack, OUTPUT);
  pinMode(13, OUTPUT);

  //Start with zero speed command to the ESCs
  ESCLeftFront.attach(addressESCLeftFront);
  ESCRightFront.attach(addressESCRightFront);
  ESCLeftBack.attach(addressESCLeftBack);
  ESCRightBack.attach(addressESCRightBack);

  ESCLeftFront.writeMicroseconds(1000);
  ESCRightFront.writeMicroseconds(1000);
  ESCLeftBack.writeMicroseconds(1000);
  ESCRightBack.writeMicroseconds(1000);

}

void loop()
{
  //Code in loop() function will execute repeatedly for as long as the CPU has power

  ////////////////////////////////////////////////////////
  //System update
  ////////////////////////////////////////////////////////

  serialPrintCounter++;
  bluetoothPrintCounter++;

  sampleTime = micros() - sampleTimeOld;
  sampleTimeMillis = (float) sampleTime / 1000;
  sampleTimeOld = micros();

  if (BLESerial.operator bool() == true) { //Turn on LED if com is ok
    digitalWrite(13, HIGH);
  }

  ////////////////////////////////////////////////////////
  //Read the sensor values
  ////////////////////////////////////////////////////////

  // To read from the gyroscope, you must first call the
  // read function. When this exits, it'll update the
  // input variables with the most current data.

  // To read from the accelerometer, you must first call the
  // read function. When this exits, it'll update the
  // input variables with the most current data.

  if (Serial.available() > 0) {

    // read the incoming byte:
    String myNumber = Serial.readString();

    if (myNumber.toInt() > 500 && myNumber.toInt() < 2500) {
      Serial.println(myNumber);
      controlSignal = myNumber.toInt();
    }
  }

  ////////////////////////////////////////////////////////
  //Control signal saturation check
  ////////////////////////////////////////////////////////

  if (controlSignal > 2000) {
    controlSignal = 2000;
  }
  else if (controlSignal < 1000) {
    controlSignal = 1000;
  }

  ////////////////////////////////////////////////////////
  //ESC control signal write
  ////////////////////////////////////////////////////////

  ESCLeftFront.writeMicroseconds(controlSignal);
  ESCRightFront.writeMicroseconds(controlSignal);
  ESCLeftBack.writeMicroseconds(controlSignal);
  ESCRightBack.writeMicroseconds(controlSignal);

  ////////////////////////////////////////////////////////
  //Print signal values
  ////////////////////////////////////////////////////////

  if (serialPrintCounter >= 999) {

    Serial.println(controlSignal);
    /*
        Serial.print("G: ");
        Serial.print(gx);
        Serial.print(", ");
        Serial.print(gy);
        Serial.print(", ");
        Serial.println(gz);
        Serial.print("A: ");
        Serial.print(ax);
        Serial.print(", ");
        Serial.print(ay);
        Serial.print(", ");
        Serial.println(az);
        Serial.print("IMU Pitch: ");
        Serial.println(sensorPitch);
        Serial.print("IMU Roll: ");
        Serial.println(sensorRoll);
        Serial.print("IMU Yaw: ");
        Serial.println(sensorYaw);
        Serial.print("Transmitter Throttle: ");
        Serial.print(transmitterThrottleRaw);
        Serial.print(", ");
        Serial.println(transmitterThrottleMapped);
        Serial.print("Transmitter Pitch: ");
        Serial.print(transmitterPitchRaw);
        Serial.print(", ");
        Serial.println(transmitterPitchMapped);
        Serial.print("Transmitter Roll: ");
        Serial.print(transmitterRollRaw);
        Serial.print(", ");
        Serial.println(transmitterRollMapped);
        Serial.print("Transmitter Yaw: ");
        Serial.print(transmitterYawRaw);
        Serial.print(", ");
        Serial.println(transmitterYawMapped);
        Serial.print("PID Pitch: ");
        Serial.print(errorPitch);
        Serial.print(", ");
        Serial.print(PpartPitch);
        Serial.print(", ");
        Serial.print(IpartPitch);
        Serial.print(", ");
        Serial.print(DpartPitch);
        Serial.print(", ");
        Serial.println(PIDPitch);
        Serial.print("PID Roll: ");
        Serial.print(errorRoll);
        Serial.print(", ");
        Serial.print(PpartRoll);
        Serial.print(", ");
        Serial.print(IpartRoll);
        Serial.print(", ");
        Serial.print(DpartRoll);
        Serial.print(", ");
        Serial.println(PIDRoll);
        Serial.print("PID Yaw: ");
        Serial.print(errorYaw);
        Serial.print(", ");
        Serial.print(PpartYaw);
        Serial.print(", ");
        Serial.print(IpartYaw);
        Serial.print(", ");
        Serial.print(DpartYaw);
        Serial.print(", ");
        Serial.println(PIDYaw);
        Serial.print("Control Signals: ");
        Serial.print(controlSignalRightFront);
        Serial.print(", ");
        Serial.print(controlSignalLeftFront);
        Serial.print(", ");
        Serial.print(controlSignalRightBack);
        Serial.print(", ");
        Serial.println(controlSignalLeftBack);
        Serial.print("Sample Time: ");
        Serial.println(sampleTimeMillis);

        //Variables for graphing
        Serial.print(transmitterThrottleMapped);
        Serial.print("SensorPitch: ");
        Serial.print(sensorPitch);
        Serial.print(" SensorRoll: ");
        Serial.print(sensorRoll);
        Serial.print(" SensorYaw: ");
        Serial.println(sensorYaw);
        Serial.print("PIDPitch: ");
        Serial.println(PIDPitch);
        Serial.print("PIDRoll: ");
        Serial.println(PIDRoll);
        Serial.print("PIDYaw: ");
        Serial.println(PIDYaw);
        Serial.print("LF: ");
        Serial.println(controlSignalLeftFront);
        Serial.print("RF: ");
        Serial.println(controlSignalRightFront);
        Serial.print("LB: ");
        Serial.println(controlSignalLeftBack);
        Serial.print("RB: ");
        Serial.println(controlSignalRightBack);
    */
    serialPrintCounter = 0;
  }

  ////////////////////////////////////////////////////////
  //Collect bluetooth data for PID parameter analyzing
  ////////////////////////////////////////////////////////

  if (bluetoothPrintCounter >= 200 && BLESerial.operator bool() == true) {
    /*
      sprintf(BLEString, "%6ld %03.01f %03.01f\n", transmitterPitchMapped, sensorPitch, PIDPitch);

      sprintf(BLEString, "%6ld %03.01f %03.01f\n", transmitterRollMapped, sensorRoll, PIDRoll);

      sprintf(BLEString, "%6ld %03.01f %03.01f\n", transmitterYawMapped, sensorYaw, PIDYaw);

      BLESerial.write(BLEString);
    */

    bluetoothPrintCounter = 0;
  }
}