From 79a41f6814de3d14fb03ceee91a5d69f58ba1fb4 Mon Sep 17 00:00:00 2001 From: Stedd Date: Thu, 19 Dec 2019 19:29:23 +0100 Subject: [PATCH] Functions --- Main/IMU.ino | 57 ++++++++++++++++++++++++++++++ Main/Main.ino | 56 +++++++++++++++++++++++------- Main/motorControl.ino | 10 ++++++ Main/motor_control.ino | 35 ------------------- Main/read_IMU.ino | 78 ------------------------------------------ 5 files changed, 110 insertions(+), 126 deletions(-) create mode 100644 Main/IMU.ino create mode 100644 Main/motorControl.ino delete mode 100644 Main/motor_control.ino delete mode 100644 Main/read_IMU.ino diff --git a/Main/IMU.ino b/Main/IMU.ino new file mode 100644 index 0000000..4869cba --- /dev/null +++ b/Main/IMU.ino @@ -0,0 +1,57 @@ +void readIMU() { + ax_ = GY85.accelerometer_x( GY85.readFromAccelerometer() ); + ay_ = GY85.accelerometer_y( GY85.readFromAccelerometer() ); + az_ = GY85.accelerometer_z( GY85.readFromAccelerometer() ); + + // cx = GY85.compass_x( GY85.readFromCompass() ); + // cy = GY85.compass_y( GY85.readFromCompass() ); + // cz = GY85.compass_z( GY85.readFromCompass() ); + + gx = GY85.gyro_x( GY85.readGyro() ); + gy = GY85.gyro_y( GY85.readGyro() ); + gz = GY85.gyro_z( GY85.readGyro() ); + // gt = GY85.temp ( GY85.readGyro() ); + + + //For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK. + if (ax_ > 60000) { + ax = (ax_ - 65535); + } + else { + ax = ax_; + } + + if (ay_ > 60000) { + ay = (ay_ - 65535); + } + else { + ay = ay_; + } + + if (az_ > 60000) { + az = (az_ - 65535); + } + else { + az = az_; + } + + acc_pitch = -1*atan2(sqrt(ay^2+az^2),ax); + + +//Serial plotter +// map(ax,-140,140,-1,1) +// map(ay,-140,140,-1,1) +// map(az,-140,140,-1,1) + + // Serial.print ( " x:" ); + Serial.print ( ax ); + Serial.print ( "," ); + Serial.print ( ay ); + Serial.print ( "," ); + Serial.print ( az ); + Serial.print ( "," ); + Serial.println ( acc_pitch*180/3.14 ); + + + +} diff --git a/Main/Main.ino b/Main/Main.ino index 6207a5e..9c73a14 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -2,12 +2,6 @@ #include "GY_85.h" #include -//Define L298N pin mappings -const int IN1 = 3; -const int IN2 = 2; -const int IN3 = 5; -const int IN4 = 4; -#define LOW 0 GY_85 GY85; //create the object @@ -15,31 +9,67 @@ int ax_, ay_, az_; int ax, ay, az; int cx, cy, cz; float gx, gy, gz, gt; +float acc_pitch; +int i, modifier; +int m1_in1 = 33; +int m1_in2 = 25; +int m2_in1 = 13; +int m2_in2 = 14; void setup() { + //Initialize serial + Serial.begin(57600); + delay(10); + //Initialice I2C // Wire.begin(SCL,SDA); Wire.begin(26, 27); delay(10); - Serial.begin(57600); - delay(10); + + //Initialize IMU GY85.init(); delay(10); - //Assign H bridge Pins - pinMode(IN1, OUTPUT); - pinMode(IN2, OUTPUT); - pinMode(IN3, OUTPUT); - pinMode(IN4, OUTPUT); + // Initialize PWM channels + // channels 0-15, resolution 1-16 bits, freq limits depend on resolution + // ledcSetup(uint8_t channel, uint32_t freq, uint8_t resolution_bits); + ledcAttachPin(m1_in1, 1); + ledcAttachPin(m1_in2, 2); + ledcAttachPin(m2_in1, 3); + ledcAttachPin(m2_in2, 4); + + ledcSetup(1, 12000, 12); // 12 kHz PWM, 8-bit resolution + ledcSetup(2, 12000, 12); + ledcSetup(3, 12000, 8); + ledcSetup(4, 12000, 8); + + i = 0; + modifier = 2; } void loop() { + //Sense readIMU(); + + + //Think + + + //Act motorControl(); + i=i+modifier; + + if (i>=4096){ + modifier = -2; + } + else if (i<=0){ + modifier = 2; + } + //Delay delay(5); // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz } diff --git a/Main/motorControl.ino b/Main/motorControl.ino new file mode 100644 index 0000000..2cf7f64 --- /dev/null +++ b/Main/motorControl.ino @@ -0,0 +1,10 @@ +void motorControl() { + //Motor 1 + ledcWrite(1, 0); + ledcWrite(2, 0); + + + //Motor 2 + // ledcWrite(3, 255); + // ledcWrite(4, 255); +} diff --git a/Main/motor_control.ino b/Main/motor_control.ino deleted file mode 100644 index e2b9040..0000000 --- a/Main/motor_control.ino +++ /dev/null @@ -1,35 +0,0 @@ -void motorControl() -{ - //Backward - if (mapValue[2] < 0) - { - if (mapValue[3] > 0) - { - analogWrite(IN3, -mapValue[2] * ((255 - mapValue[3]) / 255.0)); - analogWrite(IN1, -mapValue[2]); - } - if (mapValue[3] < 0) - { - analogWrite(IN3, -mapValue[2]); - analogWrite(IN1, -mapValue[2] * ((255 + mapValue[3]) / 255.0)); - } - analogWrite(IN2, LOW); - analogWrite(IN4, LOW); - } - - //Forward - if (mapValue[2] > 0) - { - if (mapValue[3] > 0) - { - analogWrite(IN4, mapValue[2] * ((255 - mapValue[3]) / 255.0)); - analogWrite(IN2, mapValue[2]); - } - if (mapValue[3] < 0) - { - analogWrite(IN4, mapValue[2]); - analogWrite(IN2, mapValue[2] * ((255 + mapValue[3]) / 255.0)); - } - analogWrite(IN1, LOW); - analogWrite(IN3, LOW); - } diff --git a/Main/read_IMU.ino b/Main/read_IMU.ino deleted file mode 100644 index 71818b4..0000000 --- a/Main/read_IMU.ino +++ /dev/null @@ -1,78 +0,0 @@ -void readIMU() -{ - ax_ = GY85.accelerometer_x( GY85.readFromAccelerometer() ); - ay_ = GY85.accelerometer_y( GY85.readFromAccelerometer() ); - az_ = GY85.accelerometer_z( GY85.readFromAccelerometer() ); - - cx = GY85.compass_x( GY85.readFromCompass() ); - cy = GY85.compass_y( GY85.readFromCompass() ); - cz = GY85.compass_z( GY85.readFromCompass() ); - - gx = GY85.gyro_x( GY85.readGyro() ); - gy = GY85.gyro_y( GY85.readGyro() ); - gz = GY85.gyro_z( GY85.readGyro() ); - gt = GY85.temp ( GY85.readGyro() ); - - // Serial.print ( "accelerometer" ); - // Serial.print ( " x:" ); - // Serial.print ( ax ); - // Serial.print ( " y:" ); - // Serial.print ( ay ); - // Serial.print ( " z:" ); - // Serial.print ( az ); - - //For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK. - if (ax_ > 60000) { - ax = (ax_ - 65535); - } - else { - ax = ax_; - } - - if (ay_ > 60000) { - ay = (ay_ - 65535); - } - else { - ay = ay_; - } - - if (az_ > 60000) { - az = (az_ - 65535); - } - else { - az = az_; - } - - - //Serial plotter - // Serial.print ( " x:" ); - Serial.print ( ax ); - Serial.print ( "," ); - Serial.print ( ay ); - Serial.print ( "," ); - Serial.println ( az ); - - // Serial.print(deg_R); - // Serial.print(","); - // Serial.println(deg_L); - - // Serial.print ( " compass" ); - // Serial.print ( " x:" ); - // Serial.print ( cx ); - // Serial.print ( " y:" ); - // Serial.print ( cy ); - // Serial.print (" z:"); - // Serial.print ( cz ); - - // Serial.print ( " gyro" ); - // Serial.print ( " x:" ); - // Serial.print ( gx ); - // Serial.print ( " y:" ); - // Serial.print ( gy ); - // Serial.print ( " z:" ); - // Serial.print ( gz ); - // Serial.print ( " gyro temp:" ); - // Serial.println( gt ); - - -}