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

Pitch angle+crude motor control

This commit is contained in:
Stedd 2019-12-19 20:10:09 +01:00
parent 79a41f6814
commit 0c600138c5
3 changed files with 26 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Main/IMU.ino
View File

@ -35,7 +35,7 @@ void readIMU() {
az = az_;
}
acc_pitch = -1*atan2(sqrt(ay^2+az^2),ax);
acc_pitch = -1*atan2(ax,sqrt((pow(ay,2)+pow(az,2))))*180/3.14;
//Serial plotter
@ -44,13 +44,13 @@ void readIMU() {
// 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 );
// Serial.print ( ax );
// Serial.print ( "," );
// Serial.print ( ay );
// Serial.print ( "," );
// Serial.print ( az );
// Serial.print ( "," );
// Serial.println ( acc_pitch);

5
Main/Main.ino
View File

@ -10,6 +10,7 @@ int ax, ay, az;
int cx, cy, cz;
float gx, gy, gz, gt;
float acc_pitch;
signed int speed_setp;
int i, modifier;
@ -41,8 +42,8 @@ void setup() {
ledcAttachPin(m2_in1, 3);
ledcAttachPin(m2_in2, 4);
ledcSetup(1, 12000, 12); // 12 kHz PWM, 8-bit resolution
ledcSetup(2, 12000, 12);
ledcSetup(1, 12000, 8); // 12 kHz PWM, 8-bit resolution
ledcSetup(2, 12000, 8);
ledcSetup(3, 12000, 8);
ledcSetup(4, 12000, 8);

16
Main/motorControl.ino
View File

@ -1,5 +1,19 @@
void motorControl() {
//Motor 1
// //Motor 1
// speed_setp = round(map(acc_pitch, -90.0, 90.0, -255, 255));
// if (speed_setp > 0) {
// ledcWrite(1, speed_setp);
// ledcWrite(2, 0);
// }
// else if (speed_setp < 0) {
// ledcWrite(1, 0);
// ledcWrite(2, -1*speed_setp);
// }
// Serial.print (acc_pitch);
// Serial.print ( "," );
// Serial.println(speed_setp);
ledcWrite(1, 0);
ledcWrite(2, 0);