Drew Moore / Mbed 2 deprecated Lab10Part2

Lab10 Part 2

Dependencies:   mbed

main.cpp@0:312f6a44fcac, 2016-03-30 (annotated)

Committer:
TheDoctor822
Date:
Wed Mar 30 17:21:03 2016 +0000
Revision:
0:312f6a44fcac
Suh dude

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TheDoctor822 0:312f6a44fcac 1 #include "mbed.h"
TheDoctor822 0:312f6a44fcac 2
TheDoctor822 0:312f6a44fcac 3 Serial pc(USBTX, USBRX); // tx, rx
TheDoctor822 0:312f6a44fcac 4
TheDoctor822 0:312f6a44fcac 5 AnalogIn pot1( p19 );
TheDoctor822 0:312f6a44fcac 6 PwmOut servo( p21 );
TheDoctor822 0:312f6a44fcac 7 I2C compass( p28, p27);
TheDoctor822 0:312f6a44fcac 8 Ticker tick100hz;
TheDoctor822 0:312f6a44fcac 9 Ticker tick5hz;
TheDoctor822 0:312f6a44fcac 10
TheDoctor822 0:312f6a44fcac 11 const int compass_addr = 0x42;
TheDoctor822 0:312f6a44fcac 12 char cmd[3];
TheDoctor822 0:312f6a44fcac 13 char compass_raw[2];
TheDoctor822 0:312f6a44fcac 14 float pos = 0;
TheDoctor822 0:312f6a44fcac 15 float ctrlval = 0;
TheDoctor822 0:312f6a44fcac 16 float PWM_zero = .000061;
TheDoctor822 0:312f6a44fcac 17
TheDoctor822 0:312f6a44fcac 18 float north = 0.0;
TheDoctor822 0:312f6a44fcac 19 float err, error_int;
TheDoctor822 0:312f6a44fcac 20 float error_old = 0.0;
TheDoctor822 0:312f6a44fcac 21 float error_int_old = 0.0;
TheDoctor822 0:312f6a44fcac 22 float T = 0.01;
TheDoctor822 0:312f6a44fcac 23
TheDoctor822 0:312f6a44fcac 24 float kp = 0.0002;
TheDoctor822 0:312f6a44fcac 25 float ki = 0.0;
TheDoctor822 0:312f6a44fcac 26
TheDoctor822 0:312f6a44fcac 27
TheDoctor822 0:312f6a44fcac 28 void s100hz_task(void) {
TheDoctor822 0:312f6a44fcac 29
TheDoctor822 0:312f6a44fcac 30 compass.read (compass_addr, compass_raw, 2 );
TheDoctor822 0:312f6a44fcac 31 pos = 0.1*((compass_raw[0]<<8) + compass_raw[1]) -90;
TheDoctor822 0:312f6a44fcac 32 if (pos>180) {
TheDoctor822 0:312f6a44fcac 33 pos = pos -360;
TheDoctor822 0:312f6a44fcac 34
TheDoctor822 0:312f6a44fcac 35 }
TheDoctor822 0:312f6a44fcac 36 ctrlval = pot1.read() * .05;
TheDoctor822 0:312f6a44fcac 37 err = north-pos;
TheDoctor822 0:312f6a44fcac 38 error_int = error_int_old + .5 * (err + error_old) * T ;
TheDoctor822 0:312f6a44fcac 39 ctrlval = kp * err + ki * error_int;
TheDoctor822 0:312f6a44fcac 40 ctrlval = ctrlval + PWM_zero;
TheDoctor822 0:312f6a44fcac 41 error_old = err;
TheDoctor822 0:312f6a44fcac 42 error_int_old = error_int;
TheDoctor822 0:312f6a44fcac 43
TheDoctor822 0:312f6a44fcac 44
TheDoctor822 0:312f6a44fcac 45
TheDoctor822 0:312f6a44fcac 46 servo = ctrlval;
TheDoctor822 0:312f6a44fcac 47 }
TheDoctor822 0:312f6a44fcac 48
TheDoctor822 0:312f6a44fcac 49 void s5hz_task ( void ) {
TheDoctor822 0:312f6a44fcac 50
TheDoctor822 0:312f6a44fcac 51 pc.printf ( "Position: %f, Control Duty Cycle: %f\n", pos, ctrlval );
TheDoctor822 0:312f6a44fcac 52 }
TheDoctor822 0:312f6a44fcac 53
TheDoctor822 0:312f6a44fcac 54 int main() {
TheDoctor822 0:312f6a44fcac 55
TheDoctor822 0:312f6a44fcac 56 cmd[0] = 0x47;
TheDoctor822 0:312f6a44fcac 57 cmd[1] = 0x74;
TheDoctor822 0:312f6a44fcac 58 cmd[2] = 0x72;
TheDoctor822 0:312f6a44fcac 59
TheDoctor822 0:312f6a44fcac 60 compass.write ( compass_addr, cmd, 3 );
TheDoctor822 0:312f6a44fcac 61
TheDoctor822 0:312f6a44fcac 62 tick100hz.attach ( &s100hz_task, .01 );
TheDoctor822 0:312f6a44fcac 63 tick5hz.attach ( &s5hz_task, .2 );
TheDoctor822 0:312f6a44fcac 64
TheDoctor822 0:312f6a44fcac 65 while (1) {
TheDoctor822 0:312f6a44fcac 66
TheDoctor822 0:312f6a44fcac 67 }
TheDoctor822 0:312f6a44fcac 68 }