main.cpp

00001 #include "2017_3_b.h"
00002 #include "mbed.h"
00003 #include <math.h>
00004 #include "MD_PID.h"
00005 #include "QEI.h"
00006 
00007 Omni_3 Omni(p21, p18, p11, p12, p22, p17, p27, p28, p23, p16, p29, p30, 1);
00008 
00009 Serial pc (USBTX, USBRX);
00010 I2CSlave slave(p9, p10);
00011 
00012 DigitalOut check1(LED1);
00013 
00014 union Get_data{
00015     char alldata[3];
00016     struct{
00017         signed int x :8;
00018         signed int y :8;
00019         signed int t :8;
00020     }data;
00021 }act;
00022 
00023 int main() {
00024     Omni.Drive(0, 0, 0);
00025     float x, y, st;
00026     
00027     slave.address(0x08);
00028     while(1) {
00029         int i = slave.receive();
00030         check1 = 0.0;
00031         switch (i) {
00032             case I2CSlave::ReadAddressed:    
00033                 check1 = check1 ^ 1;
00034                 break;
00035             case I2CSlave::WriteGeneral:
00036                 slave.read(act.alldata, 3);
00037                 check1 = check1 ^ 1;
00038                 break;
00039             case I2CSlave::WriteAddressed:
00040                 slave.read(act.alldata, 3);
00041                 check1 = check1 ^ 1;
00042                 x  = (float)act.data.x / 127.0;
00043                 y  = (float)act.data.y / 127.0;
00044                 st = (float)act.data.t / 127.0 * 0.1;
00045                 break;
00046         }
00047         Omni.Drive(x, y, st);
00048         
00049         for(int i = 0; i < 3; i++) act.alldata[i] = 0;
00050         
00051     }
00052 }
00053 
00054