James MacLean / Robot_Control

The subsystem design/basis for the final project

Dependencies:   mbed-rtos mbed-src pixylib

global.cpp@2:2bc519e14bae, 2016-03-13 (annotated)

Committer:
balsamfir
Date:
Sun Mar 13 17:55:42 2016 +0000
Revision:
2:2bc519e14bae
Child:
3:dfb6733ae397
First compile of big ass change

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 PwmOut leftPwm(p21);
balsamfir 2:2bc519e14bae 15 PwmOut rightPwm(p22);
balsamfir 2:2bc519e14bae 16 SPI deSpi(p5, p6, p7);
balsamfir 2:2bc519e14bae 17 Serial pc(USBTX, USBRX); // PC serial channel
balsamfir 2:2bc519e14bae 18 Serial bt(p9, p10); // Bluetooth serial channel
balsamfir 2:2bc519e14bae 19
balsamfir 2:2bc519e14bae 20 // Generic PI controller function used by the sensor control thread
balsamfir 2:2bc519e14bae 21 void PI(float *xState, float *u, float setPoint, float kP, float kI, float feedback, float bound) {
balsamfir 2:2bc519e14bae 22 float e;
balsamfir 2:2bc519e14bae 23
balsamfir 2:2bc519e14bae 24 e = setPoint - feedback;
balsamfir 2:2bc519e14bae 25
balsamfir 2:2bc519e14bae 26 // Avoid integrator wind up
balsamfir 2:2bc519e14bae 27 if((*u>=bound)||(*u<=-bound)) ; // turns off integrator when u maxed
balsamfir 2:2bc519e14bae 28 else {
balsamfir 2:2bc519e14bae 29 *xState = *xState + e;
balsamfir 2:2bc519e14bae 30 }
balsamfir 2:2bc519e14bae 31
balsamfir 2:2bc519e14bae 32 *u = kI*(*xState) + kP*e;
balsamfir 2:2bc519e14bae 33
balsamfir 2:2bc519e14bae 34 if (fabs(*u) > bound) {
balsamfir 2:2bc519e14bae 35 *u = bound;
balsamfir 2:2bc519e14bae 36 }
balsamfir 2:2bc519e14bae 37 }
balsamfir 2:2bc519e14bae 38
balsamfir 2:2bc519e14bae 39 // Converts measurements from the QE2 to rads/sec
balsamfir 2:2bc519e14bae 40 float QE2RadsPerSec(int counts, int time) {
balsamfir 2:2bc519e14bae 41 return (counts*122.62)/time;
balsamfir 2:2bc519e14bae 42 }