James MacLean / Robot_Control

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?

UserRevisionLine numberNew 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 }