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"
//PID Constants
#define KP 1.2
#define KP 0.7
#define KI 0
#define KD 0.7
#define DEADZONE_MIN 25
class DriveController{
public:
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();
float updatePid();

1
include/motor.h
View File

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

6
include/vars.h
View File

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

3
src/main.cpp
View File

@ -14,5 +14,6 @@ void loop() {
updateSensors();
//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 "vars.h"
#include <Arduino.h>
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);
}
Motor::Motor(){
}
void Motor::drive(int speed){
byte VAL_INA, VAL_INB;
if (speed == 0) {

2
src/sensors.cpp
View File

@ -3,7 +3,7 @@
void initSensors(){
compass = new DataSourceBNO055();
drive = new DriveController(new Motor(12, 11, 2, 45), new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
}
void updateSensors(){