main.cpp

00001 #include "mbed.h"
00002 #include "FastPWM.h"
00003 AnalogIn ref_raw(A3);
00004 Ticker read_adc;
00005 AnalogOut output(A2);
00006 FastPWM pwm_output(D3);
00007 DigitalOut testing(D2);
00008 
00009 double raw[5] = {0, 0, 0, 0, 0};
00010 double ref[5] = {0, 0, 0, 0, 0};
00011 /*
00012 double raw;
00013 double old_raw = 0;
00014 double old_raw_2 = 0;
00015 double old_raw_3 = 0;
00016 double old_raw_4 = 0;
00017 double ref;
00018 double old_ref = 0;
00019 double old_ref_2 = 0;
00020 double old_ref_3 = 0;
00021 double old_ref_4 = 0;
00022 */
00023 double b[5] = {0.000000018682489340283,   0.000000074729957361130,   0.000000112094936041696,   0.000000074729957361130,   0.000000018682489340283};
00024 double a[5] = {1.000000000000000,  -3.938430361819403,   5.817179417349660,  -3.819034001378269,   0.940285244767841};
00025 
00026 //double b[5] = {0.000144120224075409, 0.000576480896301634, 0.000864721344452452, 0.000576480896301634, 0.000144120224075409};
00027 //double a[5] = {1.000000000000000, -3.384972728303897, 4.337061817421342, -2.489138293812547, 0.539355128280308};
00028 
00029 void read_ref(void){
00030     //ref = 3.3*(analog_value.read()-.5);
00031     //output.write(ref_raw.read());
00032     raw[0] = .85*(ref_raw.read()-.5)+.5;
00033 //    ref = raw;
00034    
00035     ref[0] = b[0]*raw[0]+b[1]*raw[1]+b[2]*raw[2]+b[3]*raw[3]+b[4]*raw[4]-a[1]*ref[1]-a[2]*ref[2]-a[3]*ref[3]-a[4]*ref[4];
00036     
00037     for (int i=4; i > 0; i--){
00038         raw[i] = raw[i-1];
00039         ref[i] = ref[i-1];
00040         }
00041 
00042     output.write(ref[0]);
00043     pwm_output.write(ref[0]+.03);
00044     //testing=!testing;
00045     }
00046     
00047 int main() {
00048     testing=0;
00049     pwm_output.period(.000025);
00050     read_adc.attach(&read_ref, .000025);
00051     testing = 0;
00052     while(1) {
00053         //ref_raw.read();
00054         //pwm_output.write(.5);
00055         //ref_raw.read();
00056         //testing = !testing;
00057     }
00058 }