collab/pb-hal

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

Joseph RyanCompetition code2822fe3

master
5.6 KiB190 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 = 2200/26;
15double maxSpeed = .30; // 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 = 10;
39const int threshold = 35;
40void moveStraight(int dist, axis dir) {
41    cout << "Starting to move straight for dist: " << dist << endl;
42    int s, us;
43    gpioTime(0, &s, &us);
44    int ready = 0;
45    double currentTime, lastTime = (double(s) * 1000 + double(us / 1000)) / 1000;
46    for(int i = 0; i < 4; ++i) {
47        motors[i]->zero();
48        linearControl[i]->set(0);
49    }
50    if(dir == horizontal) {
51        linearControl[0]->set(dist * 1);
52        linearControl[1]->set(dist * -1);
53    } else if(dir == vertical) {
54        linearControl[2]->set(dist * 1);
55        linearControl[3]->set(dist * -1);
56    }
57    //block until you're close enough to the set point for long enough
58    while(ready < steady) {
59        // cout << "Getting there.. " << ready << "/" << steady << endl;
60    // cout << "Top: " << motors[0]->getTicks() <<
61    //      "\tBottom: " << motors[1]->getTicks() <<
62    //      "\tLeft: " << motors[2]->getTicks() <<
63    //      "\tRight: " << motors[3]->getTicks() << endl;
64        gpioTime(0, &s, &us);
65        currentTime = (double(s) * 1000 + double(us / 1000)) / 1000;
66        controlUpdate(currentTime - lastTime);
67        time_sleep(.01); // run pid around 20 times per second
68        ++ready;
69        for(int i = 0; i < 4; ++i) {
70            if(linearControl[i]->getErr() > threshold) {
71                // cout << "holdup: " << linearControl[i]->getErr() << endl;
72                ready = 0;
73                break;
74            }
75        }
76        lastTime = currentTime;
77    }
78    cout << endl << "got there" << endl;
79}
80
81static PyObject* selfTest(PyObject* self, PyObject* args) {
82    for(int i = 0; i < 4; ++i) {
83        motors[i]->set(.25);
84        time_sleep(1);
85        motors[i]->stop();
86        cout << "Motor " << i << " at " << motors[i]->getTicks() << " ticks" << endl;
87    }
88    return Py_BuildValue("i", 1);
89}
90
91static PyObject* hardStop(PyObject* self, PyObject* args) {
92    for(int i = 0; i < 4; ++i) {
93        motors[i]->stop();
94    }
95    return Py_BuildValue("i", 1);
96}
97
98static PyObject* printEnc(PyObject* self, PyObject* args) {
99    cout << "Top: " << motors[0]->getTicks() <<
100         "\tBottom: " << motors[1]->getTicks() <<
101         "\tLeft: " << motors[2]->getTicks() <<
102         "\tRight: " << motors[3]->getTicks() << endl;
103    return Py_BuildValue("i", 1);
104}
105
106static PyObject* setSpeed(PyObject* self, PyObject* args) {
107    PyArg_ParseTuple(args, "f", &maxSpeed);
108    return Py_BuildValue("i", 1);
109}
110
111static PyObject* moveDir(PyObject* self, PyObject* args) {
112    int dist, dir;
113    PyArg_ParseTuple(args, "ii", &dist, &dir);
114    cout << "moving " << dist << " units in direction: " << dir << endl;
115    dist *= ticksPerUnit;
116    switch(dir) {
117    case 0:
118        moveStraight(-dist, vertical);
119        break;
120    case 1:
121        moveStraight(dist, vertical);
122        break;
123    case 2:
124        moveStraight(-dist, horizontal);
125        break;
126    case 3:
127        moveStraight(dist, horizontal);
128        break;
129    default:
130        cout << "Invalid direction, stopping" << endl;
131        hardStop(self, args);
132        break;
133    }
134    hardStop(self, args);
135    return Py_BuildValue("i", 1);
136}
137
138static PyObject* initRobot(PyObject* self, PyObject* args) {
139    if (gpioInitialise() < 0) {
140        return Py_BuildValue("i", 0);
141    }
142    motors[3] = new DRV(15, 14, 17, 18);
143    motors[2] = new DRV(25, 8, 7, 1);
144    motors[1] = new DRV(27, 22, 10, 9);
145    motors[0] = new DRV(6, 13, 5, 0);
146    cout << "Motors initialized" << endl;
147    for(int i = 0; i < 4; ++i) {
148        linearControl[i] = new PID(.01, .01, .0003);
149    }
150    // rotationControl = new PID(p, i, d);
151    // alignmentControl = new PID(p, i, d);
152    return Py_BuildValue("i", 1);
153}
154
155static PyMethodDef halMethods[] = {
156    {"printEnc", printEnc, METH_VARARGS, NULL},
157    {"initRobot", initRobot, METH_VARARGS, NULL},
158    {"hardStop", hardStop, METH_VARARGS, NULL},
159    {"selfTest", selfTest, METH_VARARGS, NULL},
160    {"setSpeed", setSpeed, METH_VARARGS, NULL},
161    {"moveDir", moveDir, METH_VARARGS, NULL}
162};
163
164static struct PyModuleDef halmodule = {
165    PyModuleDef_HEAD_INIT,
166    "hal",
167    NULL,
168    -1,
169    halMethods
170};
171
172PyMODINIT_FUNC PyInit_hal(void) {
173    return PyModule_Create(&halmodule);
174}
175
176int main(int argc, char* argv[]) {
177    wchar_t* program = Py_DecodeLocale(argv[0], NULL);
178    if (program == NULL) {
179        fprintf(stderr, "RIP");
180        exit(1);
181    }
182    PyImport_AppendInittab("hal", PyInit_hal);
183    Py_SetProgramName(program);
184    Py_Initialize();
185    PyImport_ImportModule("hal");
186    PyMem_RawFree(program);
187    return 0;
188}
189
190