File content as of revision 1:540605f9477b:

#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;
    mot.StartQEIPut();
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;
}