Kiko Ishimoto
MyMotorDrive用の使うプログラム
Dependencies: IAP MotorClass mbed
Revision 0:2226b743b219, committed 2015-08-18
- Comitter:
- kikoaac
- Date:
- Tue Aug 18 01:31:20 2015 +0000
- Child:
- 1:540605f9477b
- Commit message:
- MotorDrive???????????
;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IAP.lib Tue Aug 18 01:31:20 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/okano/code/IAP/#c8bf974ecb33
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorClass.lib Tue Aug 18 01:31:20 2015 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/kikoaac/code/MotorClass/#842f92dcf668
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Aug 18 01:31:20 2015 +0000
@@ -0,0 +1,288 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "MotorClass.h"
+
+#include "IAP.h"
+#define addr 0xa0
+I2C i2c(p28,p27);//(p9,p10);
+Ticker tic;
+Serial pc(USBTX,USBRX);
+QEI wheel(p18,p17,NC,100,1,QEI::X4_ENCODING);
+MotorClass mot(i2c,wheel);
+IAP iap;
+char *modename[7] = {"","NomalMode","ServoMode","ServoMode(PID)","SpeedHoldMode","SpeedHoldMode(PID)","Stop"};
+char Mode;
+void InputData(char *data)
+{
+ char i=0;
+ do {
+ *(data+i)=pc.getc();
+ if(*(data+i)==127) {
+ i=0;
+ printf("\r");
+ } else {
+ printf("%c",*(data+i));
+ i++;
+ }
+ } while(*(data+i-1)!=13);
+ printf("\r\n");
+}
+void motor_select_mode_menu()
+{
+ printf("\r\nPlease Select Mode.\r\n");
+ 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");
+ printf("Back to press Q key.\r\n\n");
+ while(1) {
+ char get=pc.getc()-'0';
+ if('Q'==(get+'0')) {
+ printf("preesed Q \r\n");
+ break;
+ } else if((get>-1)&&(get<10)) {
+ if(get>6)get=6;
+ Mode = get;
+ printf("%s \r\n",modename[get]);
+ mot.Setup(get);
+ break;
+ }
+ }
+}
+void motor_setting_menu()
+{
+ char data[10];
+ printf("%s setting menu\r\n",modename[Mode]);
+
+ float a;
+ switch(Mode) {
+ case 1:
+ printf("Input Duty -1~1\r\n");
+ do {
+ InputData(data);
+ a=atof(data);
+ } while(a<-1||a>1);
+ mot=a;
+ break;
+ case 2:
+ printf("Input Target -65535~65535\r\n");
+ do {
+ InputData(data);
+ a=atoi(data);
+ } while(a<-65535/2||a>65535/2);
+ mot = a;
+ break;
+ case 3:
+ printf("Input P 0~25.5\r\n");
+ do {
+ InputData(data);
+ a=atof(data);
+ } while(a<0||a>25.5);
+ mot.SetP(a);
+ printf("Input I 0~25.5\r\n");
+ do {
+ InputData(data);
+ a=atof(data);
+ } while(a<0||a>25.5);
+ mot.SetI(a);
+ printf("Input D 0~25.5\r\n");
+ do {
+ InputData(data);
+ a=atof(data);
+ } while(a<0||a>25.5);
+ mot.SetD(atof(data));
+
+ printf("Input Target -65535~65535\r\n");
+ do {
+ InputData(data);
+ a=atoi(data);
+ } while(a<=-65535/2||a>=65535/2);
+ mot=a;
+ break;
+ case 4:
+ printf("Input Target -65535~65535\r\n");
+ do {
+ InputData(data);
+ a=atoi(data);
+ } while(a<=-65535/2||a>=65535/2);
+ mot = a;
+ break;
+ case 5:
+ printf("Input P 0~25.5\r\n");
+ do {
+ InputData(data);
+ a=atof(data);
+ } while(a<0||a>25.5);
+ mot.SetP(a);
+ printf("Input I 0~25.5\r\n");
+ do {
+ InputData(data);
+ a=atof(data);
+ } while(a<0||a>25.5);
+ mot.SetI(a);
+ printf("Input D 0~25.5\r\n");
+ do {
+ InputData(data);
+ a=atof(data);
+ } while(a<0||a>25.5);
+ mot.SetD(a);
+ printf("Input Target -65535~65535\r\n");
+ do {
+ InputData(data);
+ a=atoi(data);
+ } while(a<-65535/2||a>65535/2);
+ mot = a;
+ break;
+ case 6:
+ printf("Nothing setting\r\n");
+ }
+}
+#define Lengh 4
+char *menu[Lengh]= {"I2c slave address set","Motor setting","Debug mode","Set Use Address"};
+int main_menu(bool add_f)
+{
+ int data;
+ char *X;
+ int i=add_f;
+ while(1) {
+
+ printf("\r%s\r",menu[i]);
+ wait(0.2);
+ data=pc.getc();
+ if(data == 13)return i;
+ X=menu[i];
+ while(*X++)pc.putc(' ');
+ do {
+ if(279165==data || i<Lengh) {
+ //up
+ i++;
+ if(i==Lengh)i=0;
+ }
+ if(279166==data || i<0) {
+ //down
+ i--;
+ if(i<0)i=Lengh;
+ }
+ } while(add_f==1&&i==0);
+ }
+}
+void set_address()
+{
+ char data[10];
+ char a=2;
+ printf("Let set address.\r\n");
+ printf("2~254 *add%%2==0\r\n");
+ do {
+ InputData(data);
+ a=atoi(data);
+ } while(a<2||a>254 || a%2==1);
+ mot.UseAddress(mot.SetAddress(a));
+
+}
+void use_address()
+{
+ char a=2;
+ char data[10];
+ printf("What number set address.\r\n");
+ do {
+ InputData(data);
+ a=atoi(data);
+ } while(a<2||a>254 || a%2==1);
+ mot.UseAddress(a);
+}
+bool flagdata=0;
+bool ff=0;
+
+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"}};
+void debug_menu_mode()
+{
+ int i=0,count;
+ if(Mode==0) {
+ printf("\nNot need Debug.\r\n");
+ return;
+ }
+ i=Mode;
+ ff=1;
+ printf("\n");
+ float debug[3];
+ while(!flagdata) {
+ if(pc.readable ()) {
+ float a=0;
+ char data[10];
+ int ind=pc.getc();
+ if(ind==13)break;
+ if(ind=='p') {
+ printf("P:");
+ do {
+ InputData(data);
+ a=atof(data);
+ } while(a<0||a>25.5);
+ mot.SetP(a);
+ }
+ if(ind=='i') {
+ printf("I:");
+ do {
+ InputData(data);
+ a=atof(data);
+ } while(a<0||a>25.5);
+ mot.SetI(a);
+ }
+ if(ind=='d') {
+ printf("D:");
+ do {
+ InputData(data);
+ a=atof(data);
+ } while(a<0||a>25.5);
+ mot.SetD(a);
+ }
+ }
+ if(count==20) {
+ printf("%10s %10s %10s\r\n",debugRe[i][0],debugRe[i][1],debugRe[i][2]);
+ count=0;
+ }
+ mot.GetDebugData(debug,i);
+ printf("%10f %10f %10f\r\n",debug[0],debug[1],debug[2]);
+ wait(0.2);
+ count++;
+ }
+ ff=0;
+
+}
+int main()
+{
+ /*
+ int r;
+ r = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
+ if ( r == SECTOR_NOT_BLANK ) {
+ iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+ r = iap.erase( TARGET_SECTOR, TARGET_SECTOR );
+ }
+ iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+ r = iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
+ r = iap.compare( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
+
+ iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+ */
+ bool addr_f=0;
+main_menu_select:
+ switch(main_menu(addr_f)) {
+ case 0:
+ set_address();
+ addr_f=1;
+ mot.SetRate(1);
+ goto main_menu_select;
+ case 1:
+ goto motor_mode_select;
+ case 2:
+ goto debug_menu;
+ case 3:
+ use_address();
+ goto main_menu_select;
+ }
+debug_menu:
+ debug_menu_mode();
+ goto main_menu_select;
+motor_mode_select:
+ motor_select_mode_menu();
+ goto motor_setting;
+motor_setting:
+ motor_setting_menu();
+ goto main_menu_select;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Aug 18 01:31:20 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/8ab26030e058 \ No newline at end of file