File content as of revision 5:42c8babe5160:

#include "mbed.h"
#include "math.h"
#include "krs.h"
#include "param.h"

#define Num 4

AnalogIn Index(PA_0);
Serial pc(USBTX,USBRX);
//krs master(PC_12,PD_2,PA_7,1250000);
krs master(PA_9,PA_10,PC_7,1250000);
DigitalIn button(USER_BUTTON);
DigitalOut LED(PA_6);
void command_display();
//void finger_pose(int pose);

int main() {
    pc.printf("hello\r\n");
    LED=1;
    
    float deg=0.0f;
    int degpart[3];
    int id;
    char mode;
    while(1){
    
    command_display();
    mode=pc.getc();
    pc.printf("mode %d is selected\r\n",mode-48);
    wait(1.0);
    
    switch(mode){
    case '0':
    pc.printf("Initilizeing...\r\n");
    LED=0;
    
    master.setpos(0,1);
    //wait(0.00004);

    master.setpos(0,3);
    //wait(0.00004);
    
    master.setpos(0,4);
    //wait(0.00004);
    
    master.setpos(0,6);
    //wait(0.00004);
    
    #if 0
    for(int i=3;i<3+Num;i++){
    master.getpos(i);
    }
    #endif
    break;
    
    case '1':
    int strech=0;
    pc.printf("setting speed\r\n");
    pc.printf("setting strech\r\n");
    pc.printf("id\r\n");
    int temp[2]={0};
    for(int i=0;i<2;i++){
    temp[i]=pc.getc()-48;
    pc.printf("%d",temp[i]);
    }
    pc.printf("\r\n");
    id=temp[0]*10+temp[1];
    pc.printf("%d\r\n",id);
    pc.printf("choose strech mode from 1 to 3\r\n");
    strech=pc.getc()-48;
    
    //master.setspeed(id);
    master.setstrech(id,strech);
    
    pc.printf("degree\r\n");
    pc.printf("- or +\r\n");
    int a=pc.getc()-48;
    if(a==-3){
        a=-1;
        pc.printf("negative direction\r\n");
        }else{
            a=1;
            pc.printf("positive direction\r\n");
            }

    pc.printf("magnitude\r\n");
    for(int i=0;i<3;i++){
        degpart[i]=pc.getc()-48;
        printf("%d",degpart[i]);      
        }
        printf("\r\n");
        deg=a*(100*degpart[0]+10*degpart[1]+degpart[2]);
    
    pc.printf("%d\r\n",deg);
    
    master.setpos(deg,id);
    //getID();
    wait(0.3);
    //master.getpos(id);
    break; 
    
    case '2':
    pc.printf("enter id\r\n");
    id=pc.getc()-48;
    pc.printf("%d\r\n",id);
    wait(0.3);
    //master.getpos(id);
    break;
    
    case  '3':
    //deg_first=90.0*Index.read();
    while(button){
    pc.printf("force test extending MP\r\n");
    deg=135.0f*(Index.read()-0.5);
    int ok=0;
    
    pc.printf("press 1 to rotate next motor\r\n");
    ok=pc.getc()-48;
    if(ok==1){
    master.setpos(9+deg,6);
    ok=0;
    }
    pc.printf("press 1 to rotate next motor\r\n");
    ok=pc.getc()-48;
    if(ok==1){
    master.setpos(60+deg*0.4,5);
    ok=0;
    }pc.printf("press 1 to rotate next motor\r\n");
    ok=pc.getc()-48;
    if(ok==1){
    master.setpos(-100,4);
    ok=0;
    }
    pc.printf("press 1 to rotate next motor\r\n");
    ok=pc.getc()-48;
    if(ok==1){
    master.setpos(-79,3);
    ok=0;
    }
    //wait(0.1);
        
        
    wait(0.1);
    
    
        pc.printf("id %d angle: %d\r\n",0,master.getpos(0));
     /*   
    for(int i=3;i<3+Num;i++){
        pc.printf("id %d angle: %d\r\n",i,master.getpos(i));
        }*/
    }
    break;
    
    case '4':
    while(button){
    deg=135*(Index.read()-0.5f);
    pc.printf("analog read is %f\r\n",Index.read());
    pc.printf("force test abduction MP\r\n");
    
    master.setpos(9+deg,6);
    wait(0.1);
    //master.setpos(60,5);
    master.setpos(60+deg*0.4,5);
    wait(0.1);
    master.setpos(-100,4);
    wait(0.1);
    master.setpos(-79,3);
    wait(0.1);
    
    for(int i=3;i<3+Num;i++){
        pc.printf("id %d angle: %d\r\n",i,master.getpos(i));
        }
    }
    break;
    
    case '5':
    pc.printf("Do you want to check the ID?\r\n");
    pc.printf("y/n\r\n");
    
    if(pc.getc()=='y'){
        pc.printf("remove other motors whose ID yo don't want to know\r\n");
        wait(1.0); 
        
        pc.printf("have you removed the motors?\r\n");
        pc.printf("y/n\r\n");
        
        if(pc.getc()=='y'){
            master.getid();
            }
        }else if(pc.getc()=='n'){
            pc.printf("Do you want to change the ID\r\n");
            pc.printf("y/n\r\n");
            if(pc.getc()=='y'){
                pc.printf("remove other motors whose ID yo don't want to know\r\n");
        wait(1.0); 
        
        pc.printf("have you removed the motors?\r\n");
        pc.printf("y/n\r\n");
        
        if(pc.getc()=='y'){
            
            pc.printf("enter the id\r\n");
            id=pc.getc()-48;
            pc.printf("%d\r\n",id);
            master.setid(id);
            }
                }
            }
        
        case '6':
       
        float analog_ini=0.0f;
        int deg_ini=0;
        float deg_now=0.0f;
        float deg_past=0.0f;
        
        analog_ini=Index.read();
        pc.printf("analog initilize :%f\r\n",analog_ini);
        pc.printf("analog test\r\n");
        pc.printf("id\r\n");
        id=pc.getc()-48;
        pc.printf("ID %d test begin\r\n",id);
        deg_ini=master.getpos(id);
        pc.printf("motor %d angle is %d now\r\n",id,deg_ini);
        
        while(button){
            deg_now=(float)deg_ini-270*(Index.read()-analog_ini);
            if(deg_now>=135.0f || deg_now<=-135.0f){
                deg_now=deg_past;
                }
            pc.printf("motor angle should be %f\r\n",deg_now);
            
            if(master.setpos(deg_now,id)){
                deg_past=deg_now;
                };
            
            wait(0.3);
            master.getpos(id);
            }
        break;
        
        case '7':
         pc.printf("Setting pos to 90deg...\r\n");
    
    master.setpos(45,1);
    //wait(0.00004);

    master.setpos(45,3);
    //wait(0.00004);
    
    master.setpos(45,4);
    //wait(0.00004);
    
    master.setpos(45,6);
    //wait(0.00004);
    
    wait(0.5);
    master.setpos(0,1);
    //wait(0.00004);

    master.setpos(0,3);
    //wait(0.00004);
    
    master.setpos(0,4);
    //wait(0.00004);
    
    master.setpos(0,6);
    //wait(0.00004);
    
    break;
    
    case '8':
    pc.printf("setting slave mode\r\n");
    pc.printf("enter motor id\r\n");
    id=pc.getc()-48;
    pc.printf("setting id %d motor slave mode\r\n",id);
    master.set_slavemode(id);
    break;
    
    case '9':
     pc.printf("setting slave mode off\r\n");
    pc.printf("enter motor id\r\n");
    id=pc.getc()-48;
    pc.printf("setting id %d motor slave mode off\r\n",id);
    master.set_slavemode_off(id);
    break;
    
    default:
    break;
    }
   }  
}

void command_display(){
     pc.printf("\r\n");
     pc.printf("choose mode from 0 to 6\r\n");
     pc.printf("mode 0:Initilize position\r\n");
     pc.printf("mode 1:motor test\r\n");
     pc.printf("mode 2:motor angle now\r\n");
     pc.printf("mode 3:force test extention\r\n");
     pc.printf("mode 4:force test abduction\r\n");
     pc.printf("mode 5:id \r\n");
     pc.printf("mode 6:analog test\r\n");
     pc.printf("mode 8:set motor slave mode\r\n");
     pc.printf("mode 9:unset motor slave mode\r\n");
     pc.printf("\r\n");
     
    }