collab/pb-hal

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

Joseph RyanImprove Makefile (build object files separately)aaff13f

control
3.0 KiB118 linesraw
1#include <pigpio.h>
2#include <iostream>
3#include <csignal>
4#include <cstdlib>
5#include <vector>
6#include "../inc/motor.h"
7#include "../inc/pid.h"
8
9using namespace std;
10
11// Positive is forward/clockwise in all contexts
12// Coordinates increase in down and right directions
13// Motor numbering is 0..4 = top, bottom, left, right
14
15int loopSpeed = 50; // in ms (10 is fastest pigpio will allow)
16double maxSpeed = .6; // percentage
17
18enum axis {horizontal, vertical};
19vector<PID*> linearControl(4);
20PID* rotationControl;
21PID* alignmentControl;
22vector<Motor*> motors(4);
23
24// Run all control loops in series
25void controlCB() {
26    double gains[4];
27    for(int i = 0; i < 4; ++i) {
28        gains[i] = linearControl[i]->update(motors[i]->getTicks(),
29                                            double(loopSpeed) / 1000.);
30    }
31    // double angularCorrection = rotationControl->update(imu input here);
32    // double alignmentCorrection = alignmentControl->update(vlx input here);
33    for(int i = 0; i < 4; ++i) {
34        // optional velocity pid here to sacrifice positional precision for
35        // more easily regulated acceleration
36        motors[i]->set(max(min(gains[i], maxSpeed), -maxSpeed));
37    }
38}
39
40void moveStraight(int dist, axis dir) {
41    for(int i = 0; i < 2; ++i) {
42        int m = i + (dir == vertical);
43        motors[m]->zero();
44        linearControl[m]->set(dist * (i - 1));
45    }
46}
47
48void moveDir(int dist, int dir) {
49    switch(dir) {
50    case 0:
51        moveStraight(-dist, vertical);
52        break;
53    case 1:
54        moveStraight(dist, vertical);
55        break;
56    case 2:
57        moveStraight(-dist, horizontal);
58        break;
59    case 3:
60        moveStraight(dist, horizontal);
61        break;
62    default:
63        break;
64    }
65}
66
67void enableCB(int millis) {
68    gpioSetTimerFunc(0, millis, controlCB);
69}
70
71void disableCB() {
72    gpioSetTimerFunc(0, 0, 0);
73}
74
75void cleanup(int signum) {
76    signal(SIGINT, SIG_IGN);
77    disableCB();
78    gpioTerminate();
79    cout << "GPIO Terminated" << endl;
80    exit(0);
81}
82
83int main() {
84    if (gpioInitialise() < 0) {
85        return 1;
86    }
87    signal(SIGINT, cleanup);
88    motors[0] = new L298N(15, 14, 18, 17, 27);
89    motors[1] = new L298N(16, 20, 21, 26, 19);
90    motors[2] = new L298N(10, 9, 11, 8, 25);
91    motors[3] = new L298N(5, 6, 13, 1, 7);
92    for(int i=0;i<4;++i){
93        linearControl[i] = new PID(.008, .0005, .0003);
94    }
95    // rotationControl = new PID(p, i, d);
96    // alignmentControl = new PID(p, i, d);
97    enableCB(loopSpeed);
98    // testing pid with a single motor
99    // int setPoint = 0;
100    // while(setPoint != -1){
101    //     cout << "input setpoint: ";
102    //     cin >> setPoint;
103    //     linearControl[0]->set(setPoint);
104    // }
105
106    int testDist = 0;
107    int testDir = 0;
108    while(testDist != -1 && testDir !=-1) {
109        cout << "Input a distance in encoder ticks: ";
110        cin >> testDist;
111        cout << "Input a direction (0..4 = up down left right): ";
112        cin >> testDir;
113        moveDir(testDist, testDir);
114        // check for error conditions
115        // time_sleep(.5);
116    }
117    cleanup(SIGINT);
118}