Stedd
/
BalanceBot
1
0
Fork 0
Code Issues 6 Pull Requests Packages Projects 1 Releases Wiki Activity

Functions

This commit is contained in:
Stedd 2019-12-19 19:29:23 +01:00
parent eb11db6033
commit 79a41f6814
5 changed files with 110 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
Main/IMU.ino Normal file
View File

@ -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 );
}

56
Main/Main.ino
View File

@ -2,12 +2,6 @@
#include "GY_85.h"
#include <Wire.h>
//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
}

10
Main/motorControl.ino Normal file
View File

@ -0,0 +1,10 @@
void motorControl() {
//Motor 1
ledcWrite(1, 0);
ledcWrite(2, 0);
//Motor 2
// ledcWrite(3, 255);
// ledcWrite(4, 255);
}

35
Main/motor_control.ino
View File

@ -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);
}

78
Main/read_IMU.ino
View File

@ -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 );
}