main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "MotorClass.h"
00004 
00005 #include "IAP.h"
00006 #define addr 0xa0
00007 I2C i2c(p28,p27);//(p9,p10);
00008 Ticker tic;
00009 Serial pc(USBTX,USBRX);
00010 QEI wheel(p18,p17,NC,100,1,QEI::X4_ENCODING);
00011 MotorClass mot(i2c,wheel);
00012 IAP iap;
00013 char *modename[7] = {"","NomalMode","ServoMode","ServoMode(PID)","SpeedHoldMode","SpeedHoldMode(PID)","Stop"};
00014 char Mode;
00015 void InputData(char *data)
00016 {
00017     char i=0;
00018     do {
00019         *(data+i)=pc.getc();
00020         if(*(data+i)==127) {
00021             i=0;
00022             printf("\r");
00023         } else {
00024             printf("%c",*(data+i));
00025             i++;
00026         }
00027     } while(*(data+i-1)!=13);
00028     printf("\r\n");
00029 }
00030 void motor_select_mode_menu()
00031 {
00032     printf("\r\nPlease Select Mode.\r\n");
00033     printf("\"1\" NomalMode\r\n\"2\" ServoMode\r\n\"3\" ServoMode(PID)\r\n\"4\" SpeedHoldMode\r\n\"5\" SpeedHoldMode(PID)\r\n\"6~\" Stop\r\n");
00034     printf("Back to press Q key.\r\n\n");
00035     while(1) {
00036         char get=pc.getc()-'0';
00037         if('Q'==(get+'0')) {
00038             printf("preesed Q \r\n");
00039             break;
00040         } else if((get>-1)&&(get<10)) {
00041             if(get>6)get=6;
00042             Mode = get;
00043             printf("%s \r\n",modename[get]);
00044             mot.Setup(get);
00045             break;
00046         }
00047     }
00048 }
00049 void motor_setting_menu()
00050 {
00051     char data[10];
00052     printf("%s setting menu\r\n",modename[Mode]);
00053 
00054     float a;
00055     switch(Mode) {
00056         case 1:
00057             printf("Input Duty -1~1\r\n");
00058             do {
00059                 InputData(data);
00060                 a=atof(data);
00061             } while(a<-1||a>1);
00062             mot=a;
00063             break;
00064         case 2:
00065             printf("Input Target -65535~65535\r\n");
00066             do {
00067                 InputData(data);
00068                 a=atoi(data);
00069             } while(a<-65535/2||a>65535/2);
00070             mot = a;
00071             break;
00072         case 3:
00073             printf("Input P 0~25.5\r\n");
00074             do {
00075                 InputData(data);
00076                 a=atof(data);
00077             } while(a<0||a>25.5);
00078             mot.SetP(a);
00079             printf("Input I 0~25.5\r\n");
00080             do {
00081                 InputData(data);
00082                 a=atof(data);
00083             } while(a<0||a>25.5);
00084             mot.SetI(a);
00085             printf("Input D 0~25.5\r\n");
00086             do {
00087                 InputData(data);
00088                 a=atof(data);
00089             } while(a<0||a>25.5);
00090             mot.SetD(atof(data));
00091 
00092             printf("Input Target -65535~65535\r\n");
00093             do {
00094                 InputData(data);
00095                 a=atoi(data);
00096             } while(a<=-65535/2||a>=65535/2);
00097             mot=a;
00098             break;
00099         case 4:
00100             printf("Input Target -65535~65535\r\n");
00101             do {
00102                 InputData(data);
00103                 a=atoi(data);
00104             } while(a<=-65535/2||a>=65535/2);
00105             mot = a;
00106             break;
00107         case 5:
00108             printf("Input P 0~25.5\r\n");
00109             do {
00110                 InputData(data);
00111                 a=atof(data);
00112             } while(a<0||a>25.5);
00113             mot.SetP(a);
00114             printf("Input I 0~25.5\r\n");
00115             do {
00116                 InputData(data);
00117                 a=atof(data);
00118             } while(a<0||a>25.5);
00119             mot.SetI(a);
00120             printf("Input D 0~25.5\r\n");
00121             do {
00122                 InputData(data);
00123                 a=atof(data);
00124             } while(a<0||a>25.5);
00125             mot.SetD(a);
00126             printf("Input Target -65535~65535\r\n");
00127             do {
00128                 InputData(data);
00129                 a=atoi(data);
00130             } while(a<-65535/2||a>65535/2);
00131             mot = a;
00132             break;
00133         case 6:
00134             printf("Nothing setting\r\n");
00135     }
00136 }
00137 #define Lengh 4
00138 char *menu[Lengh]= {"I2c slave address set","Motor setting","Debug mode","Set Use Address"};
00139 int main_menu(bool add_f)
00140 {
00141     int data;
00142     char *X;
00143     int i=add_f;
00144     while(1) {
00145 
00146         printf("\r%s\r",menu[i]);
00147         wait(0.2);
00148         data=pc.getc();
00149         if(data == 13)return i;
00150         X=menu[i];
00151         while(*X++)pc.putc(' ');
00152         do {
00153             if(279165==data || i<Lengh) {
00154                 //up
00155                 i++;
00156                 if(i==Lengh)i=0;
00157             }
00158             if(279166==data || i<0) {
00159                 //down
00160                 i--;
00161                 if(i<0)i=Lengh;
00162             }
00163         } while(add_f==1&&i==0);
00164     }
00165 }
00166 void set_address()
00167 {
00168     char data[10];
00169     char a=2;
00170     printf("Let set address.\r\n");
00171     printf("2~254 *add%%2==0\r\n");
00172     do {
00173         InputData(data);
00174         a=atoi(data);
00175     } while(a<2||a>254 || a%2==1);
00176     mot.UseAddress(mot.SetAddress(a));
00177     
00178 }
00179 void use_address()
00180 {
00181     char a=2;
00182     char data[10];
00183     printf("What number set address.\r\n");
00184     do {
00185         InputData(data);
00186         a=atoi(data);
00187     } while(a<2||a>254 || a%2==1);
00188     mot.UseAddress(a);
00189 }
00190 bool flagdata=0;
00191 bool ff=0;
00192 
00193 char *debugRe[7][3]= {{},{"state","duty","No debug data"},{"point","mypoint","Difference"},{"point","mypoint","Amount of work"},{"point","mypoint","Difference"},{"point","mypoint","Amount of work"}};
00194 void debug_menu_mode()
00195 {
00196     int i=0,count;
00197     if(Mode==0) {
00198         printf("\nNot need Debug.\r\n");
00199         return;
00200     }
00201     i=Mode;
00202     ff=1;
00203     printf("\n");
00204     float debug[3];
00205     while(!flagdata) {
00206         if(pc.readable ()) {
00207             float a=0;
00208             char data[10];
00209             int ind=pc.getc();
00210             if(ind==13)break;
00211             if(ind=='p') {
00212                 printf("P:");
00213                 do {
00214                     InputData(data);
00215                     a=atof(data);
00216                 } while(a<0||a>25.5);
00217                 mot.SetP(a);
00218             }
00219             if(ind=='i') {
00220                 printf("I:");
00221                 do {
00222                     InputData(data);
00223                     a=atof(data);
00224                 } while(a<0||a>25.5);
00225                 mot.SetI(a);
00226             }
00227             if(ind=='d') {
00228                 printf("D:");
00229                 do {
00230                     InputData(data);
00231                     a=atof(data);
00232                 } while(a<0||a>25.5);
00233                 mot.SetD(a);
00234             }
00235         }
00236         if(count==20) {
00237             printf("%10s  %10s  %10s\r\n",debugRe[i][0],debugRe[i][1],debugRe[i][2]);
00238             count=0;
00239         }
00240         mot.GetDebugData(debug,i);
00241         printf("%10f  %10f  %10f\r\n",debug[0],debug[1],debug[2]);
00242         wait(0.2);
00243         count++;
00244     }
00245     ff=0;
00246 
00247 }
00248 int main()
00249 {
00250     /*
00251     int  r;
00252     r   = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
00253     if ( r == SECTOR_NOT_BLANK ) {
00254         iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
00255         r   = iap.erase( TARGET_SECTOR, TARGET_SECTOR );
00256     }
00257     iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
00258     r   = iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
00259     r   = iap.compare( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
00260 
00261     iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
00262     */
00263     bool addr_f=0;
00264     mot.StartQEIPut();
00265 main_menu_select:
00266     switch(main_menu(addr_f)) {
00267         case 0:
00268             set_address();
00269             addr_f=1;
00270             mot.SetRate(1);
00271             goto main_menu_select;
00272         case 1:
00273             goto motor_mode_select;
00274         case 2:
00275             goto debug_menu;
00276         case 3:
00277             use_address();    
00278             goto main_menu_select;
00279     }
00280 debug_menu:
00281     debug_menu_mode();
00282     goto main_menu_select;
00283 motor_mode_select:
00284     motor_select_mode_menu();
00285     goto motor_setting;
00286 motor_setting:
00287     motor_setting_menu();
00288     goto main_menu_select;
00289 }