collab/pb-hal
git clone https://git.julia.blue/collab/pb-hal
e2c64f5
tof
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 ) { 26for (int i = 0 ;i < 4 ;++ i ) { 27gains [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); 31for (int i = 0 ;i < 4 ;++ i ) { 32// optional velocity pid here to sacrifice positional precision for 33// more easily regulated acceleration 34motors [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 ) { 41int s ,us ; 42gpioTime (0 ,& s ,& us ); 43int ready = 0 ; 44double currentTime ,lastTime = (double (s )* 1000 + double (us /1000 )) /1000 ; 45for (int i = 0 ;i < 2 ;++ i ) { 46int m = i + (dir == vertical ); 47motors [m ]-> zero (); 48linearControl [m ]-> set (dist * (i - 1 )); 49 } 50//block until you're close enough to the set point for long enough 51while (ready < steady ) { 52gpioTime (0 ,& s ,& us ); 53currentTime = (double (s )* 1000 + double (us /1000 )) /1000 ; 54controlUpdate (currentTime - lastTime ); 55time_sleep (.01 );// run pid around 100 times per second 56++ ready ; 57for (int i = 0 ;i < 4 ;++ i ) { 58if (linearControl [i ]-> getErr ()> threshold ) { 59// cout << "holdup: " << linearControl[i]->getErr() << endl; 60ready = 0 ; 61break ; 62 } 63 } 64lastTime = currentTime ; 65 } 66} 67 68static PyObject * hardStop (PyObject * self ,PyObject * args ) { 69for (int i = 0 ;i < 4 ;++ i ) { 70motors [i ]-> stop (); 71 } 72return Py_BuildValue ("i" ,1 ); 73} 74 75static PyObject * moveDir (PyObject * self ,PyObject * args ) { 76int dist ,dir ; 77PyArg_ParseTuple (args ,"ii" ,& dist ,& dir ); 78cout <<"moving " <<dist <<" units in direction: " <<dir <<endl ; 79dist *=ticksPerUnit ; 80switch (dir ) { 81case 0 : 82moveStraight (- dist ,vertical ); 83break ; 84case 1 : 85moveStraight (dist ,vertical ); 86break ; 87case 2 : 88moveStraight (- dist ,horizontal ); 89break ; 90case 3 : 91moveStraight (dist ,horizontal ); 92break ; 93default : 94cout <<"Invalid direction, stopping" <<endl ; 95hardStop (self ,args ); 96break ; 97 } 98return Py_BuildValue ("i" ,1 ); 99} 100 101static PyObject * initRobot (PyObject * self ,PyObject * args ) { 102if (gpioInitialise ()< 0 ) { 103return Py_BuildValue ("i" ,0 ); 104 } 105motors [0 ]= new L298N (15 ,14 ,18 ,17 ,27 ); 106motors [1 ]= new L298N (16 ,20 ,21 ,26 ,19 ); 107motors [2 ]= new L298N (10 ,9 ,11 ,8 ,25 ); 108motors [3 ]= new L298N (5 ,6 ,13 ,1 ,7 ); 109for (int i = 0 ;i < 4 ;++ i ) { 110linearControl [i ]= new PID (.008 ,.0005 ,.0003 ); 111 } 112// rotationControl = new PID(p, i, d); 113// alignmentControl = new PID(p, i, d); 114return 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 = { 124PyModuleDef_HEAD_INIT , 125"hal" , 126NULL , 127-1 , 128halMethods 129}; 130 131PyMODINIT_FUNC PyInit_hal (void ) { 132return PyModule_Create (& halmodule ); 133} 134 135int main (int argc ,char * argv []) { 136wchar_t * program = Py_DecodeLocale (argv [0 ],NULL ); 137if (program == NULL ) { 138fprintf (stderr ,"RIP" ); 139exit (1 ); 140 } 141PyImport_AppendInittab ("hal" ,PyInit_hal ); 142Py_SetProgramName (program ); 143Py_Initialize (); 144PyImport_ImportModule ("hal" ); 145PyMem_RawFree (program ); 146return 0 ; 147} 148 149