EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Motors and recenter working

code_midgen
EmaMaker 2019-11-13 16:26:03 +01:00
parent 993383a53d
commit 146eb4d6fc
7 changed files with 35 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/drivecontroller.h
View File

@ -4,17 +4,19 @@
#include "motor.h" #include "motor.h"
//PID Constants //PID Constants
#define KP 1.2 #define KP 0.7
#define KI 0 #define KI 0
#define KD 0.7 #define KD 0.7
#define DEADZONE_MIN 25
class DriveController{ class DriveController{
public: public:
DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_); DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_);
void drive(int dir = 0, int speed = 0, int tilt = 0);
void prepareDrive(int dir = 0, int speed = 0, int tilt = 0); void drive(int dir, int speed, int tilt);
void prepareDrive(int dir, int speed, int tilt);
void drivePrepared(); void drivePrepared();
float updatePid(); float updatePid();

1
include/motor.h
View File

@ -6,6 +6,7 @@ class Motor {
public: public:
Motor(int a, int b, int pwm, int angle); Motor(int a, int b, int pwm, int angle);
Motor();
void drive(int speed); void drive(int speed);
public: public:

6
include/vars.h
View File

@ -1,2 +1,8 @@
#pragma once #pragma once
#define DEBUG_PRINT Serial #define DEBUG_PRINT Serial
#define LED_R 20
#define LED_Y 17
#define LED_G 13
#define BUZZER 30
#define SWITCH_SX 28
#define SWITCH_DX 29

28
src/drivecontroller.cpp
View File

@ -2,14 +2,10 @@
#include "sensors.h" #include "sensors.h"
DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_){ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_){
// m1 = new Motor(12, 11, 2, 45); m1 = m1_;
// m2 = new Motor(25, 24, 5, 135); m2 = m2_;
// m3 = new Motor(27, 26, 6, 225); m3 = m3_;
// m4 = new Motor(21, 22, 23, 315); m4 = m4_;
this->m1 = m1_;
this->m2 = m2_;
this->m3 = m3_;
this->m4 = m4_;
for(int i = 0; i < 360; i++){ for(int i = 0; i < 360; i++){
sins[i] = (float) sin(i); sins[i] = (float) sin(i);
@ -65,15 +61,15 @@ void DriveController::drive(int dir, int speed, int tilt){
speed4 = constrain(speed4, -255, 255); speed4 = constrain(speed4, -255, 255);
//-almost- eliminating motor deadzones, should increase motor efficiency //-almost- eliminating motor deadzones, should increase motor efficiency
speed1 = (int)speed1 > 0 ? map((int)speed1, 1, 255, 35, 255) : speed1; speed1 = (int)speed1 > 0 ? map((int)speed1, 1, 255, DEADZONE_MIN, 255) : speed1;
speed2 = (int)speed2 > 0 ? map((int)speed2, 1, 255, 35, 255) : speed2; speed2 = (int)speed2 > 0 ? map((int)speed2, 1, 255, DEADZONE_MIN, 255) : speed2;
speed3 = (int)speed3 > 0 ? map((int)speed3, 1, 255, 35, 255) : speed3; speed3 = (int)speed3 > 0 ? map((int)speed3, 1, 255, DEADZONE_MIN, 255) : speed3;
speed4 = (int)speed4 > 0 ? map((int)speed4, 1, 255, 35, 255) : speed4; speed4 = (int)speed4 > 0 ? map((int)speed4, 1, 255, DEADZONE_MIN, 255) : speed4;
speed1 = (int)speed1 < 0 ? map((int)speed1, -255, -1, -255, -35) : speed1; speed1 = (int)speed1 < 0 ? map((int)speed1, -255, -1, -255, -DEADZONE_MIN) : speed1;
speed2 = (int)speed2 < 0 ? map((int)speed2, -255, -1, -255, -35) : speed2; speed2 = (int)speed2 < 0 ? map((int)speed2, -255, -1, -255, -DEADZONE_MIN) : speed2;
speed3 = (int)speed3 < 0 ? map((int)speed3, -255, -1, -255, -35) : speed3; speed3 = (int)speed3 < 0 ? map((int)speed3, -255, -1, -255, -DEADZONE_MIN) : speed3;
speed4 = (int)speed4 < 0 ? map((int)speed4, -255, -1, -255, -35) : speed4; speed4 = (int)speed4 < 0 ? map((int)speed4, -255, -1, -255, -DEADZONE_MIN) : speed4;
m1->drive((int) speed1); m1->drive((int) speed1);
m2->drive((int) speed2); m2->drive((int) speed2);

3
src/main.cpp
View File

@ -14,5 +14,6 @@ void loop() {
updateSensors(); updateSensors();
//should recenter using predefined values //should recenter using predefined values
drive->drive(); drive->drive(0, 0, 0);
DEBUG_PRINT.println(compass->getValue());
} }

5
src/motor.cpp
View File

@ -1,4 +1,5 @@
#include "motor.h" #include "motor.h"
#include "vars.h"
#include <Arduino.h> #include <Arduino.h>
Motor::Motor(int a, int b, int pwm, int angle_){ Motor::Motor(int a, int b, int pwm, int angle_){
@ -12,6 +13,10 @@ Motor::Motor(int a, int b, int pwm, int angle_){
pinMode(pinPwm, OUTPUT); pinMode(pinPwm, OUTPUT);
} }
Motor::Motor(){
}
void Motor::drive(int speed){ void Motor::drive(int speed){
byte VAL_INA, VAL_INB; byte VAL_INA, VAL_INB;
if (speed == 0) { if (speed == 0) {