collab/pb-hal

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

Joseph RyanClean up old code and add python wrappere2c64f5

tof
4.1 KiB149 linesraw
1#include <Python.h>
2#include <pigpio.h>
3#include <iostream>
4#include <vector>
5#include "../inc/motor.h"
6#include "../inc/pid.h"
7
8using namespace std;
9
10// Positive is forward/clockwise in all contexts
11// Coordinates increase in down and right directions
12// Motor numbering is 0..4 = top, bottom, left, right
13
14const int ticksPerUnit = 400;
15const double maxSpeed = .60; // percentage
16
17enum axis {horizontal, vertical};
18vector<PID*> linearControl(4);
19PID* rotationControl;
20PID* alignmentControl;
21vector<Motor*> motors(4);
22
23// Run all control loops in series
24double gains[4];
25void controlUpdate(double elapsed) {
26    for(int i = 0; i < 4; ++i) {
27        gains[i] = linearControl[i]->update(motors[i]->getTicks(), elapsed);
28    }
29    // double angularCorrection = rotationControl->update(mpu input here);
30    // double alignmentCorrection = alignmentControl->update(vlx input here);
31    for(int i = 0; i < 4; ++i) {
32        // optional velocity pid here to sacrifice positional precision for
33        // more easily regulated acceleration
34        motors[i]->set(max(min(gains[i], maxSpeed), -maxSpeed));
35    }
36}
37
38const int steady = 5;
39const int threshold = 10;
40void moveStraight(int dist, axis dir) {
41    int s, us;
42    gpioTime(0, &s, &us);
43    int ready = 0;
44    double currentTime, lastTime = (double(s) * 1000 + double(us / 1000)) / 1000;
45    for(int i = 0; i < 2; ++i) {
46        int m = i + (dir == vertical);
47        motors[m]->zero();
48        linearControl[m]->set(dist * (i - 1));
49    }
50    //block until you're close enough to the set point for long enough
51    while(ready < steady) {
52        gpioTime(0, &s, &us);
53        currentTime = (double(s) * 1000 + double(us / 1000)) / 1000;
54        controlUpdate(currentTime - lastTime);
55        time_sleep(.01); // run pid around 100 times per second
56        ++ready;
57        for(int i = 0; i < 4; ++i) {
58            if(linearControl[i]->getErr() > threshold) {
59                // cout << "holdup: " << linearControl[i]->getErr() << endl;
60                ready = 0;
61                break;
62            }
63        }
64        lastTime = currentTime;
65    }
66}
67
68static PyObject* hardStop(PyObject* self, PyObject* args) {
69    for(int i = 0; i < 4; ++i) {
70        motors[i]->stop();
71    }
72    return Py_BuildValue("i", 1);
73}
74
75static PyObject* moveDir(PyObject* self, PyObject* args) {
76    int dist, dir;
77    PyArg_ParseTuple(args, "ii", &dist, &dir);
78    cout << "moving " << dist << " units in direction: " << dir << endl;
79    dist *= ticksPerUnit;
80    switch(dir) {
81    case 0:
82        moveStraight(-dist, vertical);
83        break;
84    case 1:
85        moveStraight(dist, vertical);
86        break;
87    case 2:
88        moveStraight(-dist, horizontal);
89        break;
90    case 3:
91        moveStraight(dist, horizontal);
92        break;
93    default:
94        cout << "Invalid direction, stopping" << endl;
95        hardStop(self, args);
96        break;
97    }
98    return Py_BuildValue("i", 1);
99}
100
101static PyObject* initRobot(PyObject* self, PyObject* args) {
102    if (gpioInitialise() < 0) {
103        return Py_BuildValue("i", 0);
104    }
105    motors[0] = new L298N(15, 14, 18, 17, 27);
106    motors[1] = new L298N(16, 20, 21, 26, 19);
107    motors[2] = new L298N(10, 9, 11, 8, 25);
108    motors[3] = new L298N(5, 6, 13, 1, 7);
109    for(int i = 0; i < 4; ++i) {
110        linearControl[i] = new PID(.008, .0005, .0003);
111    }
112    // rotationControl = new PID(p, i, d);
113    // alignmentControl = new PID(p, i, d);
114    return Py_BuildValue("i", 1);
115}
116
117static PyMethodDef halMethods[] = {
118    {"initRobot", initRobot, METH_VARARGS, NULL},
119    {"hardStop", hardStop, METH_VARARGS, NULL},
120    {"moveDir", moveDir, METH_VARARGS, NULL}
121};
122
123static struct PyModuleDef halmodule = {
124    PyModuleDef_HEAD_INIT,
125    "hal",
126    NULL,
127    -1,
128    halMethods
129};
130
131PyMODINIT_FUNC PyInit_hal(void) {
132    return PyModule_Create(&halmodule);
133}
134
135int main(int argc, char* argv[]) {
136    wchar_t* program = Py_DecodeLocale(argv[0], NULL);
137    if (program == NULL) {
138        fprintf(stderr, "RIP");
139        exit(1);
140    }
141    PyImport_AppendInittab("hal", PyInit_hal);
142    Py_SetProgramName(program);
143    Py_Initialize();
144    PyImport_ImportModule("hal");
145    PyMem_RawFree(program);
146    return 0;
147}
148
149