The subsystem design/basis for the final project
Dependencies: mbed-rtos mbed-src pixylib
global.cpp@3:dfb6733ae397, 2016-03-14 (annotated)
- Committer:
- balsamfir
- Date:
- Mon Mar 14 00:40:28 2016 +0000
- Revision:
- 3:dfb6733ae397
- Parent:
- 2:2bc519e14bae
- Child:
- 5:f655435d0782
Ported robot controls to new framework and got everything working and tested
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
balsamfir | 2:2bc519e14bae | 1 | #include "global.h" |
balsamfir | 2:2bc519e14bae | 2 | |
balsamfir | 2:2bc519e14bae | 3 | // IO Port |
balsamfir | 2:2bc519e14bae | 4 | DigitalOut led1(LED1); |
balsamfir | 2:2bc519e14bae | 5 | DigitalOut led2(LED2); |
balsamfir | 2:2bc519e14bae | 6 | DigitalOut led3(LED3); |
balsamfir | 2:2bc519e14bae | 7 | DigitalOut led4(LED4); |
balsamfir | 2:2bc519e14bae | 8 | DigitalOut leftDir(p29); |
balsamfir | 2:2bc519e14bae | 9 | DigitalOut rightDir(p30); |
balsamfir | 2:2bc519e14bae | 10 | DigitalOut spiReset(p11); |
balsamfir | 2:2bc519e14bae | 11 | DigitalOut ioReset(p12); |
balsamfir | 2:2bc519e14bae | 12 | |
balsamfir | 2:2bc519e14bae | 13 | // Comunication |
balsamfir | 2:2bc519e14bae | 14 | SPI deSpi(p5, p6, p7); |
balsamfir | 3:dfb6733ae397 | 15 | Pixy pixy(Pixy::SPI, p11, p12, p13); |
balsamfir | 2:2bc519e14bae | 16 | Serial pc(USBTX, USBRX); // PC serial channel |
balsamfir | 2:2bc519e14bae | 17 | Serial bt(p9, p10); // Bluetooth serial channel |
balsamfir | 2:2bc519e14bae | 18 | |
balsamfir | 3:dfb6733ae397 | 19 | // Other |
balsamfir | 3:dfb6733ae397 | 20 | PwmOut leftPwm(p21); |
balsamfir | 3:dfb6733ae397 | 21 | PwmOut rightPwm(p22); |
balsamfir | 3:dfb6733ae397 | 22 | InterruptIn bumper(p8); |
balsamfir | 3:dfb6733ae397 | 23 | |
balsamfir | 2:2bc519e14bae | 24 | // Generic PI controller function used by the sensor control thread |
balsamfir | 3:dfb6733ae397 | 25 | void PI(float error, float *output, float *integral, float kP, float kI, float bound) { |
balsamfir | 2:2bc519e14bae | 26 | |
balsamfir | 2:2bc519e14bae | 27 | // Avoid integrator wind up |
balsamfir | 3:dfb6733ae397 | 28 | if((*output >= bound)||(*output <= -bound)); |
balsamfir | 2:2bc519e14bae | 29 | else { |
balsamfir | 3:dfb6733ae397 | 30 | *integral = *integral + error; |
balsamfir | 2:2bc519e14bae | 31 | } |
balsamfir | 2:2bc519e14bae | 32 | |
balsamfir | 3:dfb6733ae397 | 33 | *output = kI * (*integral) + kP * error; |
balsamfir | 2:2bc519e14bae | 34 | |
balsamfir | 3:dfb6733ae397 | 35 | // Limit output to bounds |
balsamfir | 3:dfb6733ae397 | 36 | if (*output > bound) { |
balsamfir | 3:dfb6733ae397 | 37 | *output = bound; |
balsamfir | 3:dfb6733ae397 | 38 | } else if (*output < -bound) { |
balsamfir | 3:dfb6733ae397 | 39 | *output = -bound; |
balsamfir | 3:dfb6733ae397 | 40 | } |
balsamfir | 2:2bc519e14bae | 41 | } |
balsamfir | 2:2bc519e14bae | 42 | |
balsamfir | 2:2bc519e14bae | 43 | // Converts measurements from the QE2 to rads/sec |
balsamfir | 3:dfb6733ae397 | 44 | float QE2RadsPerSec(short counts, short time) { |
balsamfir | 3:dfb6733ae397 | 45 | int cnt32; |
balsamfir | 3:dfb6733ae397 | 46 | if (counts & 0x00008000) { |
balsamfir | 3:dfb6733ae397 | 47 | cnt32 = counts | 0xFFFF0000; |
balsamfir | 3:dfb6733ae397 | 48 | } else { |
balsamfir | 3:dfb6733ae397 | 49 | cnt32 = counts; |
balsamfir | 3:dfb6733ae397 | 50 | } |
balsamfir | 3:dfb6733ae397 | 51 | return (cnt32*122.62)/time; |
balsamfir | 2:2bc519e14bae | 52 | } |