Diff: main.cpp

Revision:

5:42c8babe5160

Parent:

4:b4c8d6671241

--- a/main.cpp	Sun Dec 29 16:36:37 2019 +0000
+++ b/main.cpp	Sun Dec 13 05:29:00 2020 +0000
@@ -1,71 +1,289 @@
+
 #include "mbed.h"
 #include "math.h"
 #include "krs.h"
-AnalogIn fsr(PC_0);
+#include "param.h"
+
+#define Num 4
+
+AnalogIn Index(PA_0);
 Serial pc(USBTX,USBRX);
-//krs master(PA_9,PA_10,PC_7,1250000);
-krs master(PA_9,PA_10,PC_7,112500);
+//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;
     
-    int deg=0;
-    char degpart[3];
+    float deg=0.0f;
+    int degpart[3];
     int id;
-    char a;
+    char mode;
     while(1){
-    pc.printf("choose1 or 2 or3\r\n");
-    a=pc.getc();
-    switch(a){
+    
+    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=100*degpart[0]+10*degpart[1]+degpart[2];
+        deg=a*(100*degpart[0]+10*degpart[1]+degpart[2]);
     
     pc.printf("%d\r\n",deg);
-    pc.printf("id\r\n");
-    id=pc.getc()-48;
     
-    pc.printf("%d\r\n",id);
     master.setpos(deg,id);
     //getID();
     wait(0.3);
-    master.getpos(id);
-    break;
-    
+    //master.getpos(id);
+    break; 
     
     case '2':
-    double i;
-    i=fsr.read();
-    
-    deg=i*270-135;
-    pc.printf("%d\r\n",deg);
-    master.setpos(deg,5);
+    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':
-    pc.printf("id\r\n");
-    id=pc.getc()-48;
+    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("%d\r\n",id);
-    master.getpos(id);
+    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':
-    pc.printf("w\r\n");
-    while(1){
-    master.setpos(90,5);
+    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");
     
-    master.setpos(135,6);
-    wait(0.4);
-    master.setpos(0,5);
+    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.4);
-    }
+    //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:
@@ -74,3 +292,20 @@
    }  
 }
 
+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");
+     
+    }
+
+