main.cpp

00001 #include "mbed.h"
00002 #include "omuni.h"
00003 #include "solenoid.h"
00004 
00005 #define DEBUG 0
00006 
00007 #define CON_OFFSET 15
00008 
00009 I2C i2c(p28, p27);
00010 omuni omu(&i2c, 0x10, 0x14, 0x16);
00011 solenoid sol(&i2c, 0x20);
00012 
00013 Serial con(p9, p10);
00014 Serial master(p13,p14);
00015 Serial pc(USBTX, USBRX);
00016 
00017 BusOut serialsel(p19,p20,LED1,LED2);
00018 Timer serialtimer;
00019 //Ticker readtimer;
00020 
00021 char ConData[2][12];
00022 char offset[4];
00023 
00024 /*
00025 void mbedreset()
00026 {
00027     NVIC_SystemReset();
00028 }
00029 */
00030 
00031 void GetData()
00032 {
00033     //readtimer.attach(&mbedreset, 1);
00034     static bool main_flag = 1 ;
00035     if ( con.getc() == 'H' ) {
00036          ConData[0][0] = 'H';
00037          for (int i = 1; i < 12; i++)
00038          {
00039              char t = (char)con.getc();
00040              ConData[0][i] = t;
00041          }
00042          for (int i = 0; i < 12; i++)
00043          {
00044              char t = (char)con.getc();
00045              ConData[1][i] = t;
00046          }
00047         for(int i = 0; i < 12; i++)
00048         {
00049                 master.putc(ConData[0][i]);
00050         }
00051         for(int i = 0; i < 12; i++)
00052         {
00053                 master.putc(ConData[1][i]);
00054         }
00055         if(main_flag)
00056         {
00057             /*
00058             
00059             offset[0] = ConData[0][1];
00060             offset[1] = ConData[0][2];
00061             offset[2] = ConData[1][1];
00062             offset[3] = ConData[1][2];
00063             */
00064             serialtimer.start();
00065             main_flag = 0;
00066         }
00067         //ConData[0][1] -= offset[0];
00068         //ConData[0][2] -= offset[1];
00069         //ConData[1][1] -= offset[2];
00070         //ConData[1][2] -= offset[3];
00071         
00072         if( (char)255 - CON_OFFSET < ConData[1][1] || ConData[1][1] < CON_OFFSET)
00073             ConData[1][1] = 0;
00074             
00075         serialtimer.reset();
00076     }
00077     //readtimer.detach();
00078 }
00079 
00080 
00081 int main() {
00082         serialsel = 0x05;
00083         con.baud(115200);
00084         master.baud(115200);
00085         pc.baud(115200);
00086         con.attach(&GetData,Serial::RxIrq);
00087         
00088         serialtimer.stop();
00089         serialtimer.reset();
00090         
00091         while(1){
00092             char MotorData[] = {'H', ConData[0][1], ConData[0][2], ConData[1][1], ConData[1][2]};
00093             omu.out(MotorData);
00094 
00095             char soldata = ((ConData[0][3] << 2) + ConData[1][3]) << 4;
00096             sol = soldata;
00097             
00098             if(serialtimer.read_ms() > 500)
00099             {
00100                 serialsel = ~serialsel;
00101                 serialtimer.reset();
00102                 char MotorReset[] = {'H', 0, 0, 0, 0};
00103                 omu.out(MotorReset);
00104             }
00105 
00106             #if DEBUG 
00107             for(int i = 0; i < 12; i++)
00108             {
00109                     pc.printf("%3d ",ConData[0][i]);
00110             }
00111             for(int i = 0; i < 12; i++)
00112             {
00113                     pc.printf("%3d ",ConData[1][i]);
00114             } 
00115             pc.printf("\n\r");
00116             
00117             for(int i = 0; i < 4; i++)
00118             {
00119                     pc.printf("%3d ",offset[i]);
00120             }
00121             printf("\n");
00122             
00123             printf("%d\n",soldata);
00124             #endif
00125         }  
00126 }