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