collab/pb-hal
git clone https://git.julia.blue/collab/pb-hal
aaff13f
control
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 () { 26double gains [4 ]; 27for (int i = 0 ;i < 4 ;++ i ) { 28gains [i ]= linearControl [i ]-> update (motors [i ]-> getTicks (), 29double (loopSpeed ) /1000. ); 30 } 31// double angularCorrection = rotationControl->update(imu input here); 32// double alignmentCorrection = alignmentControl->update(vlx input here); 33for (int i = 0 ;i < 4 ;++ i ) { 34// optional velocity pid here to sacrifice positional precision for 35// more easily regulated acceleration 36motors [i ]-> set (max (min (gains [i ],maxSpeed ),- maxSpeed )); 37 } 38} 39 40void moveStraight (int dist ,axis dir ) { 41for (int i = 0 ;i < 2 ;++ i ) { 42int m = i + (dir == vertical ); 43motors [m ]-> zero (); 44linearControl [m ]-> set (dist * (i - 1 )); 45 } 46} 47 48void moveDir (int dist ,int dir ) { 49switch (dir ) { 50case 0 : 51moveStraight (- dist ,vertical ); 52break ; 53case 1 : 54moveStraight (dist ,vertical ); 55break ; 56case 2 : 57moveStraight (- dist ,horizontal ); 58break ; 59case 3 : 60moveStraight (dist ,horizontal ); 61break ; 62default : 63break ; 64 } 65} 66 67void enableCB (int millis ) { 68gpioSetTimerFunc (0 ,millis ,controlCB ); 69} 70 71void disableCB () { 72gpioSetTimerFunc (0 ,0 ,0 ); 73} 74 75void cleanup (int signum ) { 76signal (SIGINT ,SIG_IGN ); 77disableCB (); 78gpioTerminate (); 79cout <<"GPIO Terminated" <<endl ; 80exit (0 ); 81} 82 83int main () { 84if (gpioInitialise ()< 0 ) { 85return 1 ; 86 } 87signal (SIGINT ,cleanup ); 88motors [0 ]= new L298N (15 ,14 ,18 ,17 ,27 ); 89motors [1 ]= new L298N (16 ,20 ,21 ,26 ,19 ); 90motors [2 ]= new L298N (10 ,9 ,11 ,8 ,25 ); 91motors [3 ]= new L298N (5 ,6 ,13 ,1 ,7 ); 92for (int i = 0 ;i < 4 ;++ i ){ 93linearControl [i ]= new PID (.008 ,.0005 ,.0003 ); 94 } 95// rotationControl = new PID(p, i, d); 96// alignmentControl = new PID(p, i, d); 97enableCB (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 106int testDist = 0 ; 107int testDir = 0 ; 108while (testDist != -1 && testDir != -1 ) { 109cout <<"Input a distance in encoder ticks: " ; 110cin >>testDist ; 111cout <<"Input a direction (0..4 = up down left right): " ; 112cin >>testDir ; 113moveDir (testDist ,testDir ); 114// check for error conditions 115// time_sleep(.5); 116 } 117cleanup (SIGINT ); 118}