collab/pb-hal

git clone https://git.julia.blue/collab/pb-hal

Joseph RyanSet up cascading PID test6c85c82

control
1.8 KiB77 linesraw
1#include <pigpio.h>
2#include <cmath>
3#include "../inc/motor.h"
4
5using namespace std;
6
7const double DEADZONE = .05; // for speed not encoder ticks
8const int FREQENCY = 440; // gotto make sure our motors are in tune
9
10L298N::L298N(int m1pin, int m2pin, int pwmpin, int encA, int encB)
11    : Motor(encA, encB), m1(m1pin), m2(m2pin), pwm(pwmpin) {
12    gpioSetMode(m1, PI_OUTPUT);
13    gpioSetMode(m2, PI_OUTPUT);
14    gpioSetMode(pwm, PI_OUTPUT);
15    gpioSetPWMfrequency(pwm, FREQENCY);   // lower freq gives lower min speed
16    gpioSetPWMrange(pwm, 1000);      // not sure why we'd want a small range
17}
18
19void L298N::set(double speed) {
20    if(speed > DEADZONE) {
21        gpioWrite(m1, 1);
22        gpioWrite(m2, 0);
23    } else if(speed < -DEADZONE) {
24        gpioWrite(m1, 0);
25        gpioWrite(m2, 1);
26    } else {
27        gpioWrite(m1, 0);
28        gpioWrite(m2, 0);
29    }
30    gpioPWM(pwm, abs(speed) * gpioGetPWMrange(pwm));
31}
32
33void L298N::stop() {
34    set(0);
35    gpioWrite(m1, 0);
36    gpioWrite(m2, 0);
37}
38
39L298N::~L298N() {
40    stop();
41}
42
43DRV::DRV(int pwmpin1, int pwmpin2, int encA, int encB)
44    : Motor(encA, encB), pwm1(pwmpin1), pwm2(pwmpin2) {
45    gpioSetMode(pwm1, PI_OUTPUT);
46    gpioSetPWMfrequency(pwm1, 440);
47    gpioSetPWMrange(pwm1, 1000);
48    gpioSetMode(pwm2, PI_OUTPUT);
49    gpioSetPWMfrequency(pwm2, 440);
50    gpioSetPWMrange(pwm2, 1000);
51}
52
53void DRV::set(double speed) {
54    double next1, next2;
55    if(speed > DEADZONE) {
56        next1 = pwm1;
57        next2 = pwm2;
58    } else if(speed < -DEADZONE) {
59        next1 = pwm2;
60        next2 = pwm1;
61    } else {
62        stop();
63        return;
64    }
65    gpioPWM(next1, abs(speed) * gpioGetPWMrange(pwm1));
66    gpioPWM(next2, 0);
67}
68
69void DRV::stop() {
70    //todo: try 0's for sleep instead of break
71    gpioWrite(pwm1, 1);
72    gpioWrite(pwm2, 1);
73}
74
75DRV::~DRV() {
76    stop();
77}