main.cpp

00001 #include "mbed.h"
00002 #include "EncoderCounter.h"
00003 #include "IMU.h"
00004 #include "Controller.h"
00005 
00006 DigitalOut myled(LED1);
00007 Serial pc1(USBTX, USBRX); // tx, rx
00008 
00009 DigitalOut led1(PB_13);
00010 DigitalOut led2(PC_8);
00011 DigitalOut led3(PC_9);
00012 DigitalOut led4(PC_0);
00013 DigitalOut led5(PC_1);
00014 DigitalOut led6(PH_1);
00015 DigitalOut led7(PC_2);
00016 DigitalOut led8(PC_3);
00017 DigitalOut led9(PB_0);
00018 
00019 // create motor control objects
00020 DigitalOut enable1(PB_1);
00021 DigitalOut enable2(PB_2);
00022 DigitalOut enable3(PB_3);
00023 
00024 PwmOut pwm1(PA_8);
00025 PwmOut pwm2(PA_9);
00026 PwmOut pwm3(PA_10);
00027 
00028 // crete Encoder read objects
00029 EncoderCounter counter1(PA_6, PC_7);
00030 EncoderCounter counter2(PB_6, PB_7);
00031 EncoderCounter counter3(PA_1, PA_0);
00032 
00033 // create IMU comunication objects
00034 SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
00035 DigitalOut csAG(PA_15);
00036 DigitalOut csM(PD_2);
00037 
00038 // escon I/O
00039 
00040 
00041 //AnalogIn M1_AOUT1(PB_4);
00042 //AnalogIn M1_AOUT2(PB_5);
00043 
00044 //AnalogIn M2_AOUT1(PB_14);
00045 //AnalogIn M2_AOUT2(PB_15);
00046 
00047 AnalogIn M3_AOUT1(PA_7);
00048 //AnalogIn M3_AOUT2(PA_11);
00049 
00050 
00051 Thread thread;
00052 
00053 int main()
00054 {
00055     //IMU
00056     IMU imu(spi, csAG, csM);
00057 
00058     pwm1.period_us(200);
00059     enable1 = 1;
00060     //pwm1.write(0.6f);
00061 
00062     pwm2.period_us(200);
00063     enable2 = 1;
00064     //pwm2.write(0.6f);
00065 
00066     pwm3.period_us(200);
00067     enable3 = 1;
00068     //pwm3.write(0.6f);
00069     
00070     //controller
00071     Controller controller(pwm1,pwm2,pwm3,counter1,counter2,counter3,imu);
00072     
00073     pc1.baud(100000);
00074     
00075     led1 = 0;
00076     led2 = 0;
00077     led3 = 0;
00078     led4 = 0;
00079 
00080     int i = 1;
00081     
00082     float gamma_z = 0;
00083     float gz_vor = 0;
00084     
00085     int t = 0;
00086     
00087     while(1) {
00088         
00089         led4 = 0;
00090         led9 = 0;
00091         led5 = 0;
00092         led6 = 0;
00093         led7 = 0;
00094         led8 = 0;
00095 
00096         switch(i) {
00097             case 1:
00098                 led4 = 1;
00099                 break;
00100 
00101             case 2:
00102                 led9 = 1;
00103                 break;
00104 
00105             case 3:
00106                 led5 = 1;
00107                 break;
00108 
00109             case 4:
00110                 led6 = 1;
00111                 break;
00112 
00113             case 5:
00114                 led7 = 1;
00115                 break;
00116 
00117             case 6:
00118                 led8 = 1;
00119                 break;
00120 
00121             case 7:
00122                 led7 = 1;
00123                 break;
00124 
00125             case 8:
00126                 led6 = 1;
00127                 break;
00128 
00129             case 9:
00130                 led5 = 1;
00131                 break;
00132             case 10:
00133                 led9 = 1;
00134                 break;
00135 
00136             default:
00137                 led1 = 0;
00138                 led2 = 0;
00139                 led3 = 0;
00140                 led4 = 0;
00141                 break;
00142         }
00143 
00144         i++;
00145         if(i==11) {
00146             i=1;
00147         }
00148 
00149         myled = 1; // LED is ON
00150         thread.wait(50.0);
00151         
00152         float gz = imu.readGyroZ();
00153         gamma_z = (gz-gz_vor)*0.05f + gamma_z;
00154         float f = t*(-0.0011f);
00155         
00156         //printf("counter1: %d counter2: %d counter3: %d\r\n", counter1.read(),counter2.read(),counter3.read());
00157         //printf("%.5f %.5f\r\n",imu.getGammaX(),imu.getGammaY());
00158         //printf("%.5f %.5f\r\n",imu.getGammaZ(),imu.getGammaZ());
00159         //printf("%.5f %.5f\r\n",imu.readMagnetometerX()*166.0045f -550.0f, imu.readMagnetometerY()*140.6528f + 0.0f);
00160         //printf("%.2f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f\r\n",time,imu.readAccelerationX(), imu.readAccelerationY(), imu.readAccelerationZ(),imu.readGyroX(), imu.readGyroY(), imu.readGyroZ(),imu.readMagnetometerX(), imu.readMagnetometerY(), imu.readMagnetometerZ());
00161         
00162         //printf("%.7f %.7f\r\n",M1_AOUT1.read(),M1_AOUT2.read());
00163         
00164         //printf("main\r\n");
00165         
00166         t++;
00167 
00168     }
00169 }