Kiko Ishimoto / Mbed 2 deprecated MotorDriveDEBUG

MyMotorDrive用の使うプログラム

Dependencies:   IAP MotorClass mbed

main.cpp@1:540605f9477b, 2015-08-18 (annotated)

Committer:
kikoaac
Date:
Tue Aug 18 04:58:26 2015 +0000
Revision:
1:540605f9477b
Parent:
0:2226b743b219 
var2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kikoaac 0:2226b743b219 1 #include "mbed.h"
kikoaac 0:2226b743b219 2 #include "QEI.h"
kikoaac 0:2226b743b219 3 #include "MotorClass.h"
kikoaac 0:2226b743b219 4
kikoaac 0:2226b743b219 5 #include "IAP.h"
kikoaac 0:2226b743b219 6 #define addr 0xa0
kikoaac 0:2226b743b219 7 I2C i2c(p28,p27);//(p9,p10);
kikoaac 0:2226b743b219 8 Ticker tic;
kikoaac 0:2226b743b219 9 Serial pc(USBTX,USBRX);
kikoaac 0:2226b743b219 10 QEI wheel(p18,p17,NC,100,1,QEI::X4_ENCODING);
kikoaac 0:2226b743b219 11 MotorClass mot(i2c,wheel);
kikoaac 0:2226b743b219 12 IAP iap;
kikoaac 0:2226b743b219 13 char *modename[7] = {"","NomalMode","ServoMode","ServoMode(PID)","SpeedHoldMode","SpeedHoldMode(PID)","Stop"};
kikoaac 0:2226b743b219 14 char Mode;
kikoaac 0:2226b743b219 15 void InputData(char *data)
kikoaac 0:2226b743b219 16 {
kikoaac 0:2226b743b219 17 char i=0;
kikoaac 0:2226b743b219 18 do {
kikoaac 0:2226b743b219 19 *(data+i)=pc.getc();
kikoaac 0:2226b743b219 20 if(*(data+i)==127) {
kikoaac 0:2226b743b219 21 i=0;
kikoaac 0:2226b743b219 22 printf("\r");
kikoaac 0:2226b743b219 23 } else {
kikoaac 0:2226b743b219 24 printf("%c",*(data+i));
kikoaac 0:2226b743b219 25 i++;
kikoaac 0:2226b743b219 26 }
kikoaac 0:2226b743b219 27 } while(*(data+i-1)!=13);
kikoaac 0:2226b743b219 28 printf("\r\n");
kikoaac 0:2226b743b219 29 }
kikoaac 0:2226b743b219 30 void motor_select_mode_menu()
kikoaac 0:2226b743b219 31 {
kikoaac 0:2226b743b219 32 printf("\r\nPlease Select Mode.\r\n");
kikoaac 0:2226b743b219 33 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");
kikoaac 0:2226b743b219 34 printf("Back to press Q key.\r\n\n");
kikoaac 0:2226b743b219 35 while(1) {
kikoaac 0:2226b743b219 36 char get=pc.getc()-'0';
kikoaac 0:2226b743b219 37 if('Q'==(get+'0')) {
kikoaac 0:2226b743b219 38 printf("preesed Q \r\n");
kikoaac 0:2226b743b219 39 break;
kikoaac 0:2226b743b219 40 } else if((get>-1)&&(get<10)) {
kikoaac 0:2226b743b219 41 if(get>6)get=6;
kikoaac 0:2226b743b219 42 Mode = get;
kikoaac 0:2226b743b219 43 printf("%s \r\n",modename[get]);
kikoaac 0:2226b743b219 44 mot.Setup(get);
kikoaac 0:2226b743b219 45 break;
kikoaac 0:2226b743b219 46 }
kikoaac 0:2226b743b219 47 }
kikoaac 0:2226b743b219 48 }
kikoaac 0:2226b743b219 49 void motor_setting_menu()
kikoaac 0:2226b743b219 50 {
kikoaac 0:2226b743b219 51 char data[10];
kikoaac 0:2226b743b219 52 printf("%s setting menu\r\n",modename[Mode]);
kikoaac 0:2226b743b219 53
kikoaac 0:2226b743b219 54 float a;
kikoaac 0:2226b743b219 55 switch(Mode) {
kikoaac 0:2226b743b219 56 case 1:
kikoaac 0:2226b743b219 57 printf("Input Duty -1~1\r\n");
kikoaac 0:2226b743b219 58 do {
kikoaac 0:2226b743b219 59 InputData(data);
kikoaac 0:2226b743b219 60 a=atof(data);
kikoaac 0:2226b743b219 61 } while(a<-1||a>1);
kikoaac 0:2226b743b219 62 mot=a;
kikoaac 0:2226b743b219 63 break;
kikoaac 0:2226b743b219 64 case 2:
kikoaac 0:2226b743b219 65 printf("Input Target -65535~65535\r\n");
kikoaac 0:2226b743b219 66 do {
kikoaac 0:2226b743b219 67 InputData(data);
kikoaac 0:2226b743b219 68 a=atoi(data);
kikoaac 0:2226b743b219 69 } while(a<-65535/2||a>65535/2);
kikoaac 0:2226b743b219 70 mot = a;
kikoaac 0:2226b743b219 71 break;
kikoaac 0:2226b743b219 72 case 3:
kikoaac 0:2226b743b219 73 printf("Input P 0~25.5\r\n");
kikoaac 0:2226b743b219 74 do {
kikoaac 0:2226b743b219 75 InputData(data);
kikoaac 0:2226b743b219 76 a=atof(data);
kikoaac 0:2226b743b219 77 } while(a<0||a>25.5);
kikoaac 0:2226b743b219 78 mot.SetP(a);
kikoaac 0:2226b743b219 79 printf("Input I 0~25.5\r\n");
kikoaac 0:2226b743b219 80 do {
kikoaac 0:2226b743b219 81 InputData(data);
kikoaac 0:2226b743b219 82 a=atof(data);
kikoaac 0:2226b743b219 83 } while(a<0||a>25.5);
kikoaac 0:2226b743b219 84 mot.SetI(a);
kikoaac 0:2226b743b219 85 printf("Input D 0~25.5\r\n");
kikoaac 0:2226b743b219 86 do {
kikoaac 0:2226b743b219 87 InputData(data);
kikoaac 0:2226b743b219 88 a=atof(data);
kikoaac 0:2226b743b219 89 } while(a<0||a>25.5);
kikoaac 0:2226b743b219 90 mot.SetD(atof(data));
kikoaac 0:2226b743b219 91
kikoaac 0:2226b743b219 92 printf("Input Target -65535~65535\r\n");
kikoaac 0:2226b743b219 93 do {
kikoaac 0:2226b743b219 94 InputData(data);
kikoaac 0:2226b743b219 95 a=atoi(data);
kikoaac 0:2226b743b219 96 } while(a<=-65535/2||a>=65535/2);
kikoaac 0:2226b743b219 97 mot=a;
kikoaac 0:2226b743b219 98 break;
kikoaac 0:2226b743b219 99 case 4:
kikoaac 0:2226b743b219 100 printf("Input Target -65535~65535\r\n");
kikoaac 0:2226b743b219 101 do {
kikoaac 0:2226b743b219 102 InputData(data);
kikoaac 0:2226b743b219 103 a=atoi(data);
kikoaac 0:2226b743b219 104 } while(a<=-65535/2||a>=65535/2);
kikoaac 0:2226b743b219 105 mot = a;
kikoaac 0:2226b743b219 106 break;
kikoaac 0:2226b743b219 107 case 5:
kikoaac 0:2226b743b219 108 printf("Input P 0~25.5\r\n");
kikoaac 0:2226b743b219 109 do {
kikoaac 0:2226b743b219 110 InputData(data);
kikoaac 0:2226b743b219 111 a=atof(data);
kikoaac 0:2226b743b219 112 } while(a<0||a>25.5);
kikoaac 0:2226b743b219 113 mot.SetP(a);
kikoaac 0:2226b743b219 114 printf("Input I 0~25.5\r\n");
kikoaac 0:2226b743b219 115 do {
kikoaac 0:2226b743b219 116 InputData(data);
kikoaac 0:2226b743b219 117 a=atof(data);
kikoaac 0:2226b743b219 118 } while(a<0||a>25.5);
kikoaac 0:2226b743b219 119 mot.SetI(a);
kikoaac 0:2226b743b219 120 printf("Input D 0~25.5\r\n");
kikoaac 0:2226b743b219 121 do {
kikoaac 0:2226b743b219 122 InputData(data);
kikoaac 0:2226b743b219 123 a=atof(data);
kikoaac 0:2226b743b219 124 } while(a<0||a>25.5);
kikoaac 0:2226b743b219 125 mot.SetD(a);
kikoaac 0:2226b743b219 126 printf("Input Target -65535~65535\r\n");
kikoaac 0:2226b743b219 127 do {
kikoaac 0:2226b743b219 128 InputData(data);
kikoaac 0:2226b743b219 129 a=atoi(data);
kikoaac 0:2226b743b219 130 } while(a<-65535/2||a>65535/2);
kikoaac 0:2226b743b219 131 mot = a;
kikoaac 0:2226b743b219 132 break;
kikoaac 0:2226b743b219 133 case 6:
kikoaac 0:2226b743b219 134 printf("Nothing setting\r\n");
kikoaac 0:2226b743b219 135 }
kikoaac 0:2226b743b219 136 }
kikoaac 0:2226b743b219 137 #define Lengh 4
kikoaac 0:2226b743b219 138 char *menu[Lengh]= {"I2c slave address set","Motor setting","Debug mode","Set Use Address"};
kikoaac 0:2226b743b219 139 int main_menu(bool add_f)
kikoaac 0:2226b743b219 140 {
kikoaac 0:2226b743b219 141 int data;
kikoaac 0:2226b743b219 142 char *X;
kikoaac 0:2226b743b219 143 int i=add_f;
kikoaac 0:2226b743b219 144 while(1) {
kikoaac 0:2226b743b219 145
kikoaac 0:2226b743b219 146 printf("\r%s\r",menu[i]);
kikoaac 0:2226b743b219 147 wait(0.2);
kikoaac 0:2226b743b219 148 data=pc.getc();
kikoaac 0:2226b743b219 149 if(data == 13)return i;
kikoaac 0:2226b743b219 150 X=menu[i];
kikoaac 0:2226b743b219 151 while(*X++)pc.putc(' ');
kikoaac 0:2226b743b219 152 do {
kikoaac 0:2226b743b219 153 if(279165==data || i<Lengh) {
kikoaac 0:2226b743b219 154 //up
kikoaac 0:2226b743b219 155 i++;
kikoaac 0:2226b743b219 156 if(i==Lengh)i=0;
kikoaac 0:2226b743b219 157 }
kikoaac 0:2226b743b219 158 if(279166==data || i<0) {
kikoaac 0:2226b743b219 159 //down
kikoaac 0:2226b743b219 160 i--;
kikoaac 0:2226b743b219 161 if(i<0)i=Lengh;
kikoaac 0:2226b743b219 162 }
kikoaac 0:2226b743b219 163 } while(add_f==1&&i==0);
kikoaac 0:2226b743b219 164 }
kikoaac 0:2226b743b219 165 }
kikoaac 0:2226b743b219 166 void set_address()
kikoaac 0:2226b743b219 167 {
kikoaac 0:2226b743b219 168 char data[10];
kikoaac 0:2226b743b219 169 char a=2;
kikoaac 0:2226b743b219 170 printf("Let set address.\r\n");
kikoaac 0:2226b743b219 171 printf("2~254 *add%%2==0\r\n");
kikoaac 0:2226b743b219 172 do {
kikoaac 0:2226b743b219 173 InputData(data);
kikoaac 0:2226b743b219 174 a=atoi(data);
kikoaac 0:2226b743b219 175 } while(a<2||a>254 || a%2==1);
kikoaac 0:2226b743b219 176 mot.UseAddress(mot.SetAddress(a));
kikoaac 0:2226b743b219 177
kikoaac 0:2226b743b219 178 }
kikoaac 0:2226b743b219 179 void use_address()
kikoaac 0:2226b743b219 180 {
kikoaac 0:2226b743b219 181 char a=2;
kikoaac 0:2226b743b219 182 char data[10];
kikoaac 0:2226b743b219 183 printf("What number set address.\r\n");
kikoaac 0:2226b743b219 184 do {
kikoaac 0:2226b743b219 185 InputData(data);
kikoaac 0:2226b743b219 186 a=atoi(data);
kikoaac 0:2226b743b219 187 } while(a<2||a>254 || a%2==1);
kikoaac 0:2226b743b219 188 mot.UseAddress(a);
kikoaac 0:2226b743b219 189 }
kikoaac 0:2226b743b219 190 bool flagdata=0;
kikoaac 0:2226b743b219 191 bool ff=0;
kikoaac 0:2226b743b219 192
kikoaac 0:2226b743b219 193 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"}};
kikoaac 0:2226b743b219 194 void debug_menu_mode()
kikoaac 0:2226b743b219 195 {
kikoaac 0:2226b743b219 196 int i=0,count;
kikoaac 0:2226b743b219 197 if(Mode==0) {
kikoaac 0:2226b743b219 198 printf("\nNot need Debug.\r\n");
kikoaac 0:2226b743b219 199 return;
kikoaac 0:2226b743b219 200 }
kikoaac 0:2226b743b219 201 i=Mode;
kikoaac 0:2226b743b219 202 ff=1;
kikoaac 0:2226b743b219 203 printf("\n");
kikoaac 0:2226b743b219 204 float debug[3];
kikoaac 0:2226b743b219 205 while(!flagdata) {
kikoaac 0:2226b743b219 206 if(pc.readable ()) {
kikoaac 0:2226b743b219 207 float a=0;
kikoaac 0:2226b743b219 208 char data[10];
kikoaac 0:2226b743b219 209 int ind=pc.getc();
kikoaac 0:2226b743b219 210 if(ind==13)break;
kikoaac 0:2226b743b219 211 if(ind=='p') {
kikoaac 0:2226b743b219 212 printf("P:");
kikoaac 0:2226b743b219 213 do {
kikoaac 0:2226b743b219 214 InputData(data);
kikoaac 0:2226b743b219 215 a=atof(data);
kikoaac 0:2226b743b219 216 } while(a<0||a>25.5);
kikoaac 0:2226b743b219 217 mot.SetP(a);
kikoaac 0:2226b743b219 218 }
kikoaac 0:2226b743b219 219 if(ind=='i') {
kikoaac 0:2226b743b219 220 printf("I:");
kikoaac 0:2226b743b219 221 do {
kikoaac 0:2226b743b219 222 InputData(data);
kikoaac 0:2226b743b219 223 a=atof(data);
kikoaac 0:2226b743b219 224 } while(a<0||a>25.5);
kikoaac 0:2226b743b219 225 mot.SetI(a);
kikoaac 0:2226b743b219 226 }
kikoaac 0:2226b743b219 227 if(ind=='d') {
kikoaac 0:2226b743b219 228 printf("D:");
kikoaac 0:2226b743b219 229 do {
kikoaac 0:2226b743b219 230 InputData(data);
kikoaac 0:2226b743b219 231 a=atof(data);
kikoaac 0:2226b743b219 232 } while(a<0||a>25.5);
kikoaac 0:2226b743b219 233 mot.SetD(a);
kikoaac 0:2226b743b219 234 }
kikoaac 0:2226b743b219 235 }
kikoaac 0:2226b743b219 236 if(count==20) {
kikoaac 0:2226b743b219 237 printf("%10s %10s %10s\r\n",debugRe[i][0],debugRe[i][1],debugRe[i][2]);
kikoaac 0:2226b743b219 238 count=0;
kikoaac 0:2226b743b219 239 }
kikoaac 0:2226b743b219 240 mot.GetDebugData(debug,i);
kikoaac 0:2226b743b219 241 printf("%10f %10f %10f\r\n",debug[0],debug[1],debug[2]);
kikoaac 0:2226b743b219 242 wait(0.2);
kikoaac 0:2226b743b219 243 count++;
kikoaac 0:2226b743b219 244 }
kikoaac 0:2226b743b219 245 ff=0;
kikoaac 0:2226b743b219 246
kikoaac 0:2226b743b219 247 }
kikoaac 0:2226b743b219 248 int main()
kikoaac 0:2226b743b219 249 {
kikoaac 0:2226b743b219 250 /*
kikoaac 0:2226b743b219 251 int r;
kikoaac 0:2226b743b219 252 r = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
kikoaac 0:2226b743b219 253 if ( r == SECTOR_NOT_BLANK ) {
kikoaac 0:2226b743b219 254 iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
kikoaac 0:2226b743b219 255 r = iap.erase( TARGET_SECTOR, TARGET_SECTOR );
kikoaac 0:2226b743b219 256 }
kikoaac 0:2226b743b219 257 iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
kikoaac 0:2226b743b219 258 r = iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
kikoaac 0:2226b743b219 259 r = iap.compare( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
kikoaac 0:2226b743b219 260
kikoaac 0:2226b743b219 261 iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
kikoaac 0:2226b743b219 262 */
kikoaac 0:2226b743b219 263 bool addr_f=0;
kikoaac 1:540605f9477b 264 mot.StartQEIPut();
kikoaac 0:2226b743b219 265 main_menu_select:
kikoaac 0:2226b743b219 266 switch(main_menu(addr_f)) {
kikoaac 0:2226b743b219 267 case 0:
kikoaac 0:2226b743b219 268 set_address();
kikoaac 0:2226b743b219 269 addr_f=1;
kikoaac 0:2226b743b219 270 mot.SetRate(1);
kikoaac 0:2226b743b219 271 goto main_menu_select;
kikoaac 0:2226b743b219 272 case 1:
kikoaac 0:2226b743b219 273 goto motor_mode_select;
kikoaac 0:2226b743b219 274 case 2:
kikoaac 0:2226b743b219 275 goto debug_menu;
kikoaac 0:2226b743b219 276 case 3:
kikoaac 0:2226b743b219 277 use_address();
kikoaac 0:2226b743b219 278 goto main_menu_select;
kikoaac 0:2226b743b219 279 }
kikoaac 0:2226b743b219 280 debug_menu:
kikoaac 0:2226b743b219 281 debug_menu_mode();
kikoaac 0:2226b743b219 282 goto main_menu_select;
kikoaac 0:2226b743b219 283 motor_mode_select:
kikoaac 0:2226b743b219 284 motor_select_mode_menu();
kikoaac 0:2226b743b219 285 goto motor_setting;
kikoaac 0:2226b743b219 286 motor_setting:
kikoaac 0:2226b743b219 287 motor_setting_menu();
kikoaac 0:2226b743b219 288 goto main_menu_select;
kikoaac 0:2226b743b219 289 }