#include <Servo.h>
float i;
float k;
float j;
float roll;
float yaw;
float pitch;
const float referenceVolts = 5.0; // the default reference on a 5-volt board
const int batteryPin = 0; // battery is connected to analog pin 0
Servo myservo;
Servo myservo1;
Servo myservo2;
Servo myservo3;
int in1 = 3; //PORT
int in2 = 6; //PORT
int in3 = 2; //STBD
int in4 = 4; //STBD
int in8 = 5; //STBD PWM
int in5 = 12; //MAIN
int in6 = 13; //MAIN
int in7 = 11; //MAIN PWM
void setup() {
Serial.begin(9600);
myservo.attach(8);
myservo1.attach(9);
myservo2.attach(10);
myservo3.attach(7);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
pinMode(in5, OUTPUT);
pinMode(in6, OUTPUT);
pinMode(in7, OUTPUT);
myservo.write(90);
myservo1.write(90);
myservo2.write(90);
myservo3.write(90);
}
void TurnOFFA() {
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
digitalWrite(in5, LOW);
digitalWrite(in6, LOW);
}
void loop() {
while (Serial.available() > 0) {
i = Serial.parseFloat();
j = Serial.parseFloat();
k = Serial.parseFloat();
roll = Serial.parseFloat();
pitch = Serial.parseFloat();
yaw = Serial.parseFloat();
float Vi = i;
float Vj = j;
float Vk = k;
float PosMag = (pow((pow(i, 2)) + (pow(j, 2)) + (pow(k, 2)), 0.5));
float PosVec[] = {(i / PosMag), (j / PosMag), (k / PosMag)};
float Speed = (pow((pow(Vi, 2)) + (pow(Vj, 2)) + (pow(Vk, 2)), 0.5));
float SpeedMag;
if (Speed > 100) {
SpeedMag = 255;
}
if (Speed < 100) {
SpeedMag = map(Speed, 0, 100, 0, 255);
}
float SpeedVec[] = {(i / SpeedMag), (j / SpeedMag), (k / SpeedMag)};
int theta = degrees(atan(PosVec[1] / PosVec[0])) + 90 + map(pitch, -100, 100, -90, 90);
int hyp = (pow((pow(PosVec[0], 2)) + (pow(PosVec[1], 2)), 0.5));
int psi = map(k, -100, 100, -90, 90) + 90 + map(roll, -100, 100, -90, 90);
float theta1 = 180 - abs(theta);
float psi1 = 180 - abs(psi);
float theta2 = 360 - abs(theta);
float psi2 = 360 - abs(psi);
Above is a section of the full code it is to long to post it all but it is mostly this if statement repeated.
Thank you for the tip, I had not yet read that. The "< >" are to be used because characters were being dropped during communication between the controller and the Arduino. It should check that I am getting a "good signal". When I insert "100,0,0,0,0,0" into the serial monitor it accepts the input and runs. Once I add the "<100,0,0,0,0,0> it no longer accepts the input, and nothing runs.