Surgical_Hand / Mbed 2 deprecated krs3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed krs3

main.cpp@5:42c8babe5160, 2020-12-13 (annotated)

Committer:
takaoha
Date:
Sun Dec 13 05:29:00 2020 +0000
Revision:
5:42c8babe5160
Parent:
4:b4c8d6671241 
kondo_servo_code; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takaoha 5:42c8babe5160 1
Soutajin 0:651a188c5dc3 2 #include "mbed.h"
takaoha 1:fa3a640a55e0 3 #include "math.h"
takaoha 3:e356b3e7ecfd 4 #include "krs.h"
takaoha 5:42c8babe5160 5 #include "param.h"
takaoha 5:42c8babe5160 6
takaoha 5:42c8babe5160 7 #define Num 4
takaoha 5:42c8babe5160 8
takaoha 5:42c8babe5160 9 AnalogIn Index(PA_0);
takaoha 2:33681dfc2aa5 10 Serial pc(USBTX,USBRX);
takaoha 5:42c8babe5160 11 //krs master(PC_12,PD_2,PA_7,1250000);
takaoha 5:42c8babe5160 12 krs master(PA_9,PA_10,PC_7,1250000);
takaoha 5:42c8babe5160 13 DigitalIn button(USER_BUTTON);
takaoha 5:42c8babe5160 14 DigitalOut LED(PA_6);
takaoha 5:42c8babe5160 15 void command_display();
takaoha 5:42c8babe5160 16 //void finger_pose(int pose);
takaoha 5:42c8babe5160 17
Soutajin 0:651a188c5dc3 18 int main() {
takaoha 1:fa3a640a55e0 19 pc.printf("hello\r\n");
takaoha 5:42c8babe5160 20 LED=1;
takaoha 1:fa3a640a55e0 21
takaoha 5:42c8babe5160 22 float deg=0.0f;
takaoha 5:42c8babe5160 23 int degpart[3];
takaoha 1:fa3a640a55e0 24 int id;
takaoha 5:42c8babe5160 25 char mode;
takaoha 1:fa3a640a55e0 26 while(1){
takaoha 5:42c8babe5160 27
takaoha 5:42c8babe5160 28 command_display();
takaoha 5:42c8babe5160 29 mode=pc.getc();
takaoha 5:42c8babe5160 30 pc.printf("mode %d is selected\r\n",mode-48);
takaoha 5:42c8babe5160 31 wait(1.0);
takaoha 5:42c8babe5160 32
takaoha 5:42c8babe5160 33 switch(mode){
takaoha 5:42c8babe5160 34 case '0':
takaoha 5:42c8babe5160 35 pc.printf("Initilizeing...\r\n");
takaoha 5:42c8babe5160 36 LED=0;
takaoha 5:42c8babe5160 37
takaoha 5:42c8babe5160 38 master.setpos(0,1);
takaoha 5:42c8babe5160 39 //wait(0.00004);
takaoha 5:42c8babe5160 40
takaoha 5:42c8babe5160 41 master.setpos(0,3);
takaoha 5:42c8babe5160 42 //wait(0.00004);
takaoha 5:42c8babe5160 43
takaoha 5:42c8babe5160 44 master.setpos(0,4);
takaoha 5:42c8babe5160 45 //wait(0.00004);
takaoha 5:42c8babe5160 46
takaoha 5:42c8babe5160 47 master.setpos(0,6);
takaoha 5:42c8babe5160 48 //wait(0.00004);
takaoha 5:42c8babe5160 49
takaoha 5:42c8babe5160 50 #if 0
takaoha 5:42c8babe5160 51 for(int i=3;i<3+Num;i++){
takaoha 5:42c8babe5160 52 master.getpos(i);
takaoha 5:42c8babe5160 53 }
takaoha 5:42c8babe5160 54 #endif
takaoha 5:42c8babe5160 55 break;
takaoha 5:42c8babe5160 56
takaoha 2:33681dfc2aa5 57 case '1':
takaoha 5:42c8babe5160 58 int strech=0;
takaoha 5:42c8babe5160 59 pc.printf("setting speed\r\n");
takaoha 5:42c8babe5160 60 pc.printf("setting strech\r\n");
takaoha 5:42c8babe5160 61 pc.printf("id\r\n");
takaoha 5:42c8babe5160 62 int temp[2]={0};
takaoha 5:42c8babe5160 63 for(int i=0;i<2;i++){
takaoha 5:42c8babe5160 64 temp[i]=pc.getc()-48;
takaoha 5:42c8babe5160 65 pc.printf("%d",temp[i]);
takaoha 5:42c8babe5160 66 }
takaoha 5:42c8babe5160 67 pc.printf("\r\n");
takaoha 5:42c8babe5160 68 id=temp[0]*10+temp[1];
takaoha 5:42c8babe5160 69 pc.printf("%d\r\n",id);
takaoha 5:42c8babe5160 70 pc.printf("choose strech mode from 1 to 3\r\n");
takaoha 5:42c8babe5160 71 strech=pc.getc()-48;
takaoha 5:42c8babe5160 72
takaoha 5:42c8babe5160 73 //master.setspeed(id);
takaoha 5:42c8babe5160 74 master.setstrech(id,strech);
takaoha 5:42c8babe5160 75
takaoha 1:fa3a640a55e0 76 pc.printf("degree\r\n");
takaoha 5:42c8babe5160 77 pc.printf("- or +\r\n");
takaoha 5:42c8babe5160 78 int a=pc.getc()-48;
takaoha 5:42c8babe5160 79 if(a==-3){
takaoha 5:42c8babe5160 80 a=-1;
takaoha 5:42c8babe5160 81 pc.printf("negative direction\r\n");
takaoha 5:42c8babe5160 82 }else{
takaoha 5:42c8babe5160 83 a=1;
takaoha 5:42c8babe5160 84 pc.printf("positive direction\r\n");
takaoha 5:42c8babe5160 85 }
takaoha 5:42c8babe5160 86
takaoha 5:42c8babe5160 87 pc.printf("magnitude\r\n");
takaoha 1:fa3a640a55e0 88 for(int i=0;i<3;i++){
takaoha 1:fa3a640a55e0 89 degpart[i]=pc.getc()-48;
takaoha 1:fa3a640a55e0 90 printf("%d",degpart[i]);
takaoha 1:fa3a640a55e0 91 }
takaoha 1:fa3a640a55e0 92 printf("\r\n");
takaoha 5:42c8babe5160 93 deg=a*(100*degpart[0]+10*degpart[1]+degpart[2]);
takaoha 3:e356b3e7ecfd 94
takaoha 1:fa3a640a55e0 95 pc.printf("%d\r\n",deg);
takaoha 3:e356b3e7ecfd 96
takaoha 3:e356b3e7ecfd 97 master.setpos(deg,id);
takaoha 1:fa3a640a55e0 98 //getID();
takaoha 4:b4c8d6671241 99 wait(0.3);
takaoha 5:42c8babe5160 100 //master.getpos(id);
takaoha 5:42c8babe5160 101 break;
takaoha 2:33681dfc2aa5 102
takaoha 2:33681dfc2aa5 103 case '2':
takaoha 5:42c8babe5160 104 pc.printf("enter id\r\n");
takaoha 5:42c8babe5160 105 id=pc.getc()-48;
takaoha 5:42c8babe5160 106 pc.printf("%d\r\n",id);
takaoha 5:42c8babe5160 107 wait(0.3);
takaoha 5:42c8babe5160 108 //master.getpos(id);
takaoha 2:33681dfc2aa5 109 break;
takaoha 2:33681dfc2aa5 110
takaoha 5:42c8babe5160 111 case '3':
takaoha 5:42c8babe5160 112 //deg_first=90.0*Index.read();
takaoha 5:42c8babe5160 113 while(button){
takaoha 5:42c8babe5160 114 pc.printf("force test extending MP\r\n");
takaoha 5:42c8babe5160 115 deg=135.0f*(Index.read()-0.5);
takaoha 5:42c8babe5160 116 int ok=0;
takaoha 3:e356b3e7ecfd 117
takaoha 5:42c8babe5160 118 pc.printf("press 1 to rotate next motor\r\n");
takaoha 5:42c8babe5160 119 ok=pc.getc()-48;
takaoha 5:42c8babe5160 120 if(ok==1){
takaoha 5:42c8babe5160 121 master.setpos(9+deg,6);
takaoha 5:42c8babe5160 122 ok=0;
takaoha 5:42c8babe5160 123 }
takaoha 5:42c8babe5160 124 pc.printf("press 1 to rotate next motor\r\n");
takaoha 5:42c8babe5160 125 ok=pc.getc()-48;
takaoha 5:42c8babe5160 126 if(ok==1){
takaoha 5:42c8babe5160 127 master.setpos(60+deg*0.4,5);
takaoha 5:42c8babe5160 128 ok=0;
takaoha 5:42c8babe5160 129 }pc.printf("press 1 to rotate next motor\r\n");
takaoha 5:42c8babe5160 130 ok=pc.getc()-48;
takaoha 5:42c8babe5160 131 if(ok==1){
takaoha 5:42c8babe5160 132 master.setpos(-100,4);
takaoha 5:42c8babe5160 133 ok=0;
takaoha 5:42c8babe5160 134 }
takaoha 5:42c8babe5160 135 pc.printf("press 1 to rotate next motor\r\n");
takaoha 5:42c8babe5160 136 ok=pc.getc()-48;
takaoha 5:42c8babe5160 137 if(ok==1){
takaoha 5:42c8babe5160 138 master.setpos(-79,3);
takaoha 5:42c8babe5160 139 ok=0;
takaoha 5:42c8babe5160 140 }
takaoha 5:42c8babe5160 141 //wait(0.1);
takaoha 5:42c8babe5160 142
takaoha 5:42c8babe5160 143
takaoha 5:42c8babe5160 144 wait(0.1);
takaoha 5:42c8babe5160 145
takaoha 5:42c8babe5160 146
takaoha 5:42c8babe5160 147 pc.printf("id %d angle: %d\r\n",0,master.getpos(0));
takaoha 5:42c8babe5160 148 /*
takaoha 5:42c8babe5160 149 for(int i=3;i<3+Num;i++){
takaoha 5:42c8babe5160 150 pc.printf("id %d angle: %d\r\n",i,master.getpos(i));
takaoha 5:42c8babe5160 151 }*/
takaoha 5:42c8babe5160 152 }
takaoha 2:33681dfc2aa5 153 break;
takaoha 2:33681dfc2aa5 154
takaoha 4:b4c8d6671241 155 case '4':
takaoha 5:42c8babe5160 156 while(button){
takaoha 5:42c8babe5160 157 deg=135*(Index.read()-0.5f);
takaoha 5:42c8babe5160 158 pc.printf("analog read is %f\r\n",Index.read());
takaoha 5:42c8babe5160 159 pc.printf("force test abduction MP\r\n");
takaoha 5:42c8babe5160 160
takaoha 5:42c8babe5160 161 master.setpos(9+deg,6);
takaoha 5:42c8babe5160 162 wait(0.1);
takaoha 5:42c8babe5160 163 //master.setpos(60,5);
takaoha 5:42c8babe5160 164 master.setpos(60+deg*0.4,5);
takaoha 5:42c8babe5160 165 wait(0.1);
takaoha 5:42c8babe5160 166 master.setpos(-100,4);
takaoha 5:42c8babe5160 167 wait(0.1);
takaoha 5:42c8babe5160 168 master.setpos(-79,3);
takaoha 5:42c8babe5160 169 wait(0.1);
takaoha 5:42c8babe5160 170
takaoha 5:42c8babe5160 171 for(int i=3;i<3+Num;i++){
takaoha 5:42c8babe5160 172 pc.printf("id %d angle: %d\r\n",i,master.getpos(i));
takaoha 5:42c8babe5160 173 }
takaoha 5:42c8babe5160 174 }
takaoha 5:42c8babe5160 175 break;
takaoha 5:42c8babe5160 176
takaoha 5:42c8babe5160 177 case '5':
takaoha 5:42c8babe5160 178 pc.printf("Do you want to check the ID?\r\n");
takaoha 5:42c8babe5160 179 pc.printf("y/n\r\n");
takaoha 4:b4c8d6671241 180
takaoha 5:42c8babe5160 181 if(pc.getc()=='y'){
takaoha 5:42c8babe5160 182 pc.printf("remove other motors whose ID yo don't want to know\r\n");
takaoha 5:42c8babe5160 183 wait(1.0);
takaoha 5:42c8babe5160 184
takaoha 5:42c8babe5160 185 pc.printf("have you removed the motors?\r\n");
takaoha 5:42c8babe5160 186 pc.printf("y/n\r\n");
takaoha 5:42c8babe5160 187
takaoha 5:42c8babe5160 188 if(pc.getc()=='y'){
takaoha 5:42c8babe5160 189 master.getid();
takaoha 5:42c8babe5160 190 }
takaoha 5:42c8babe5160 191 }else if(pc.getc()=='n'){
takaoha 5:42c8babe5160 192 pc.printf("Do you want to change the ID\r\n");
takaoha 5:42c8babe5160 193 pc.printf("y/n\r\n");
takaoha 5:42c8babe5160 194 if(pc.getc()=='y'){
takaoha 5:42c8babe5160 195 pc.printf("remove other motors whose ID yo don't want to know\r\n");
takaoha 5:42c8babe5160 196 wait(1.0);
takaoha 5:42c8babe5160 197
takaoha 5:42c8babe5160 198 pc.printf("have you removed the motors?\r\n");
takaoha 5:42c8babe5160 199 pc.printf("y/n\r\n");
takaoha 5:42c8babe5160 200
takaoha 5:42c8babe5160 201 if(pc.getc()=='y'){
takaoha 5:42c8babe5160 202
takaoha 5:42c8babe5160 203 pc.printf("enter the id\r\n");
takaoha 5:42c8babe5160 204 id=pc.getc()-48;
takaoha 5:42c8babe5160 205 pc.printf("%d\r\n",id);
takaoha 5:42c8babe5160 206 master.setid(id);
takaoha 5:42c8babe5160 207 }
takaoha 5:42c8babe5160 208 }
takaoha 5:42c8babe5160 209 }
takaoha 5:42c8babe5160 210
takaoha 5:42c8babe5160 211 case '6':
takaoha 5:42c8babe5160 212
takaoha 5:42c8babe5160 213 float analog_ini=0.0f;
takaoha 5:42c8babe5160 214 int deg_ini=0;
takaoha 5:42c8babe5160 215 float deg_now=0.0f;
takaoha 5:42c8babe5160 216 float deg_past=0.0f;
takaoha 5:42c8babe5160 217
takaoha 5:42c8babe5160 218 analog_ini=Index.read();
takaoha 5:42c8babe5160 219 pc.printf("analog initilize :%f\r\n",analog_ini);
takaoha 5:42c8babe5160 220 pc.printf("analog test\r\n");
takaoha 5:42c8babe5160 221 pc.printf("id\r\n");
takaoha 5:42c8babe5160 222 id=pc.getc()-48;
takaoha 5:42c8babe5160 223 pc.printf("ID %d test begin\r\n",id);
takaoha 5:42c8babe5160 224 deg_ini=master.getpos(id);
takaoha 5:42c8babe5160 225 pc.printf("motor %d angle is %d now\r\n",id,deg_ini);
takaoha 5:42c8babe5160 226
takaoha 5:42c8babe5160 227 while(button){
takaoha 5:42c8babe5160 228 deg_now=(float)deg_ini-270*(Index.read()-analog_ini);
takaoha 5:42c8babe5160 229 if(deg_now>=135.0f || deg_now<=-135.0f){
takaoha 5:42c8babe5160 230 deg_now=deg_past;
takaoha 5:42c8babe5160 231 }
takaoha 5:42c8babe5160 232 pc.printf("motor angle should be %f\r\n",deg_now);
takaoha 5:42c8babe5160 233
takaoha 5:42c8babe5160 234 if(master.setpos(deg_now,id)){
takaoha 5:42c8babe5160 235 deg_past=deg_now;
takaoha 5:42c8babe5160 236 };
takaoha 5:42c8babe5160 237
takaoha 5:42c8babe5160 238 wait(0.3);
takaoha 5:42c8babe5160 239 master.getpos(id);
takaoha 5:42c8babe5160 240 }
takaoha 5:42c8babe5160 241 break;
takaoha 5:42c8babe5160 242
takaoha 5:42c8babe5160 243 case '7':
takaoha 5:42c8babe5160 244 pc.printf("Setting pos to 90deg...\r\n");
takaoha 5:42c8babe5160 245
takaoha 5:42c8babe5160 246 master.setpos(45,1);
takaoha 5:42c8babe5160 247 //wait(0.00004);
takaoha 5:42c8babe5160 248
takaoha 5:42c8babe5160 249 master.setpos(45,3);
takaoha 5:42c8babe5160 250 //wait(0.00004);
takaoha 5:42c8babe5160 251
takaoha 5:42c8babe5160 252 master.setpos(45,4);
takaoha 5:42c8babe5160 253 //wait(0.00004);
takaoha 5:42c8babe5160 254
takaoha 5:42c8babe5160 255 master.setpos(45,6);
takaoha 5:42c8babe5160 256 //wait(0.00004);
takaoha 5:42c8babe5160 257
takaoha 5:42c8babe5160 258 wait(0.5);
takaoha 5:42c8babe5160 259 master.setpos(0,1);
takaoha 5:42c8babe5160 260 //wait(0.00004);
takaoha 5:42c8babe5160 261
takaoha 5:42c8babe5160 262 master.setpos(0,3);
takaoha 5:42c8babe5160 263 //wait(0.00004);
takaoha 5:42c8babe5160 264
takaoha 5:42c8babe5160 265 master.setpos(0,4);
takaoha 5:42c8babe5160 266 //wait(0.00004);
takaoha 4:b4c8d6671241 267
takaoha 4:b4c8d6671241 268 master.setpos(0,6);
takaoha 5:42c8babe5160 269 //wait(0.00004);
takaoha 5:42c8babe5160 270
takaoha 5:42c8babe5160 271 break;
takaoha 5:42c8babe5160 272
takaoha 5:42c8babe5160 273 case '8':
takaoha 5:42c8babe5160 274 pc.printf("setting slave mode\r\n");
takaoha 5:42c8babe5160 275 pc.printf("enter motor id\r\n");
takaoha 5:42c8babe5160 276 id=pc.getc()-48;
takaoha 5:42c8babe5160 277 pc.printf("setting id %d motor slave mode\r\n",id);
takaoha 5:42c8babe5160 278 master.set_slavemode(id);
takaoha 5:42c8babe5160 279 break;
takaoha 5:42c8babe5160 280
takaoha 5:42c8babe5160 281 case '9':
takaoha 5:42c8babe5160 282 pc.printf("setting slave mode off\r\n");
takaoha 5:42c8babe5160 283 pc.printf("enter motor id\r\n");
takaoha 5:42c8babe5160 284 id=pc.getc()-48;
takaoha 5:42c8babe5160 285 pc.printf("setting id %d motor slave mode off\r\n",id);
takaoha 5:42c8babe5160 286 master.set_slavemode_off(id);
takaoha 4:b4c8d6671241 287 break;
takaoha 4:b4c8d6671241 288
takaoha 2:33681dfc2aa5 289 default:
takaoha 2:33681dfc2aa5 290 break;
takaoha 2:33681dfc2aa5 291 }
takaoha 3:e356b3e7ecfd 292 }
takaoha 1:fa3a640a55e0 293 }
takaoha 1:fa3a640a55e0 294
takaoha 5:42c8babe5160 295 void command_display(){
takaoha 5:42c8babe5160 296 pc.printf("\r\n");
takaoha 5:42c8babe5160 297 pc.printf("choose mode from 0 to 6\r\n");
takaoha 5:42c8babe5160 298 pc.printf("mode 0:Initilize position\r\n");
takaoha 5:42c8babe5160 299 pc.printf("mode 1:motor test\r\n");
takaoha 5:42c8babe5160 300 pc.printf("mode 2:motor angle now\r\n");
takaoha 5:42c8babe5160 301 pc.printf("mode 3:force test extention\r\n");
takaoha 5:42c8babe5160 302 pc.printf("mode 4:force test abduction\r\n");
takaoha 5:42c8babe5160 303 pc.printf("mode 5:id \r\n");
takaoha 5:42c8babe5160 304 pc.printf("mode 6:analog test\r\n");
takaoha 5:42c8babe5160 305 pc.printf("mode 8:set motor slave mode\r\n");
takaoha 5:42c8babe5160 306 pc.printf("mode 9:unset motor slave mode\r\n");
takaoha 5:42c8babe5160 307 pc.printf("\r\n");
takaoha 5:42c8babe5160 308
takaoha 5:42c8babe5160 309 }
takaoha 5:42c8babe5160 310
takaoha 5:42c8babe5160 311