surgical_hand2020 / 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

krs.cpp@6:2f9e3f56b101, 2020-12-13 (annotated)

Committer:
takaoha
Date:
Sun Dec 13 06:07:22 2020 +0000
Revision:
6:2f9e3f56b101
Parent:
5:42c8babe5160
Child:
7:c8bf191a325d
finger_joint_motor; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takaoha 3:e356b3e7ecfd 1 #include "mbed.h"
takaoha 3:e356b3e7ecfd 2 #include "krs.h"
takaoha 5:42c8babe5160 3 #include "param.h"
takaoha 3:e356b3e7ecfd 4
takaoha 3:e356b3e7ecfd 5 krs::krs(PinName tx, PinName rx, PinName io, int baud) : master(tx,rx),en(io)
takaoha 3:e356b3e7ecfd 6 {
takaoha 3:e356b3e7ecfd 7 master.baud(baud);
takaoha 3:e356b3e7ecfd 8 if(baud==112500){
takaoha 3:e356b3e7ecfd 9 waittime=0.0002;
takaoha 3:e356b3e7ecfd 10 }else if(baud==1250000){
takaoha 3:e356b3e7ecfd 11 waittime=0.00002;
takaoha 5:42c8babe5160 12
takaoha 3:e356b3e7ecfd 13 }
takaoha 3:e356b3e7ecfd 14 master.format(8, Serial::Even, 1);
takaoha 3:e356b3e7ecfd 15 en.output();
takaoha 3:e356b3e7ecfd 16 }
takaoha 3:e356b3e7ecfd 17
takaoha 5:42c8babe5160 18 bool krs::setpos(float deg,int id){
takaoha 3:e356b3e7ecfd 19 txlen=3;
takaoha 3:e356b3e7ecfd 20 rxlen=3;
takaoha 3:e356b3e7ecfd 21 int txdata[txlen];
takaoha 3:e356b3e7ecfd 22 int rxdata[rxlen];
takaoha 3:e356b3e7ecfd 23 en=1;
takaoha 3:e356b3e7ecfd 24
takaoha 5:42c8babe5160 25 if(deg>=135 || deg<=-135){
takaoha 3:e356b3e7ecfd 26 printf("failed\r\n");
takaoha 3:e356b3e7ecfd 27 return 0;
takaoha 3:e356b3e7ecfd 28 }
takaoha 3:e356b3e7ecfd 29 int pos=deg*29.633;
takaoha 3:e356b3e7ecfd 30 pos=pos+7500;
takaoha 3:e356b3e7ecfd 31
takaoha 3:e356b3e7ecfd 32 txdata[0]=0x80+id;
takaoha 3:e356b3e7ecfd 33 txdata[1]=((pos >> 7) & 0x007F);
takaoha 3:e356b3e7ecfd 34 txdata[2]=(pos & 0x007F);
takaoha 3:e356b3e7ecfd 35
takaoha 3:e356b3e7ecfd 36 for(int i=0;i<txlen;i++){
takaoha 3:e356b3e7ecfd 37 master.putc(txdata[i]);
takaoha 3:e356b3e7ecfd 38 }
takaoha 3:e356b3e7ecfd 39 wait(waittime);
takaoha 3:e356b3e7ecfd 40 //wait(0.00002);//1250000bps用
takaoha 3:e356b3e7ecfd 41 //wait(0.0002);
takaoha 5:42c8babe5160 42 #if 1
takaoha 3:e356b3e7ecfd 43 en=0;
takaoha 3:e356b3e7ecfd 44 for(int i=0;i<rxlen;i++){
takaoha 3:e356b3e7ecfd 45 rxdata[i]=master.getc();
takaoha 3:e356b3e7ecfd 46 }
takaoha 5:42c8babe5160 47 #if 1
takaoha 3:e356b3e7ecfd 48 int degnow;
takaoha 3:e356b3e7ecfd 49 degnow=((rxdata[1] << 7) & 0x3F80) + (rxdata[2] & 0x007F);
takaoha 3:e356b3e7ecfd 50 degnow=(degnow-7500)/29.6;
takaoha 3:e356b3e7ecfd 51
takaoha 5:42c8babe5160 52
takaoha 5:42c8babe5160 53 printf("%d\r\n",rxdata[0]);
takaoha 5:42c8babe5160 54 printf("%d\r\n",rxdata[1]);
takaoha 5:42c8babe5160 55 printf("%d\r\n",rxdata[2]);
takaoha 5:42c8babe5160 56 printf("角度%d\r\n",degnow);
takaoha 5:42c8babe5160 57 #endif
takaoha 5:42c8babe5160 58 #endif
takaoha 3:e356b3e7ecfd 59 }
takaoha 4:b4c8d6671241 60
takaoha 5:42c8babe5160 61 int krs::getpos(int id){
takaoha 4:b4c8d6671241 62 en=1;
takaoha 3:e356b3e7ecfd 63 txlen=2;
takaoha 3:e356b3e7ecfd 64 rxlen=4;
takaoha 3:e356b3e7ecfd 65 unsigned char txdata[txlen];
takaoha 3:e356b3e7ecfd 66 unsigned char rxdata[rxlen];
takaoha 3:e356b3e7ecfd 67 int degnow;
takaoha 3:e356b3e7ecfd 68
takaoha 3:e356b3e7ecfd 69 txdata[0]=0xA0+id;
takaoha 3:e356b3e7ecfd 70 txdata[1]=0x05;
takaoha 3:e356b3e7ecfd 71
takaoha 3:e356b3e7ecfd 72
takaoha 4:b4c8d6671241 73 for(int i=0;i<txlen;i++){
takaoha 3:e356b3e7ecfd 74 master.putc(txdata[i]);
takaoha 3:e356b3e7ecfd 75 }
takaoha 3:e356b3e7ecfd 76 wait(waittime);
takaoha 4:b4c8d6671241 77 //wait(0.00002);
takaoha 3:e356b3e7ecfd 78 en=0;
takaoha 3:e356b3e7ecfd 79
takaoha 4:b4c8d6671241 80 for(int i=0;i<rxlen;i++){
takaoha 3:e356b3e7ecfd 81 rxdata[i]=master.getc();
takaoha 3:e356b3e7ecfd 82 }
takaoha 3:e356b3e7ecfd 83
takaoha 5:42c8babe5160 84 printf("motor ID: %d",rxdata[0]&0x1F);
takaoha 3:e356b3e7ecfd 85 degnow=((rxdata[2] << 7) & 0x3F80) + (rxdata[3] & 0x007F);
takaoha 3:e356b3e7ecfd 86 degnow=(degnow-7500)/29.6;
takaoha 5:42c8babe5160 87 printf(" angle:%d\r\n",degnow);
takaoha 5:42c8babe5160 88 return degnow;
takaoha 3:e356b3e7ecfd 89 }
takaoha 4:b4c8d6671241 90
takaoha 5:42c8babe5160 91 void krs::getid(){
takaoha 5:42c8babe5160 92
takaoha 5:42c8babe5160 93 txlen=4;
takaoha 5:42c8babe5160 94 rxlen=1;
takaoha 5:42c8babe5160 95 unsigned char txdata[txlen];
takaoha 5:42c8babe5160 96 unsigned char rxdata[rxlen];
takaoha 5:42c8babe5160 97
takaoha 5:42c8babe5160 98 en=1;
takaoha 5:42c8babe5160 99 txdata[0]=0xFF;
takaoha 5:42c8babe5160 100 txdata[1]=0x00;
takaoha 5:42c8babe5160 101 txdata[2]=0x00;
takaoha 5:42c8babe5160 102 txdata[3]=0x00;
takaoha 5:42c8babe5160 103
takaoha 5:42c8babe5160 104 for(int i=0;i<txlen;i++){
takaoha 5:42c8babe5160 105 master.putc(txdata[i]);
takaoha 5:42c8babe5160 106 }
takaoha 5:42c8babe5160 107 wait(waittime);
takaoha 5:42c8babe5160 108 en=0;
takaoha 5:42c8babe5160 109
takaoha 5:42c8babe5160 110 for(int i=0;i<rxlen;i++){
takaoha 5:42c8babe5160 111 rxdata[i]=master.getc()&0x1F;
takaoha 5:42c8babe5160 112 }
takaoha 5:42c8babe5160 113 printf("motor id is %d\r\n",rxdata[0]);
takaoha 5:42c8babe5160 114 }
takaoha 5:42c8babe5160 115
takaoha 5:42c8babe5160 116 void krs::setid(int id){
takaoha 5:42c8babe5160 117 txlen=4;
takaoha 5:42c8babe5160 118 rxlen=1;
takaoha 5:42c8babe5160 119 unsigned char txdata[txlen];
takaoha 5:42c8babe5160 120 unsigned char rxdata[rxlen];
takaoha 5:42c8babe5160 121
takaoha 5:42c8babe5160 122 en=1;
takaoha 5:42c8babe5160 123 txdata[0]=0xE0 + id;
takaoha 5:42c8babe5160 124 txdata[1]=0x01;
takaoha 5:42c8babe5160 125 txdata[2]=0x01;
takaoha 5:42c8babe5160 126 txdata[3]=0x01;
takaoha 5:42c8babe5160 127
takaoha 5:42c8babe5160 128 for(int i=0;i<txlen;i++){
takaoha 5:42c8babe5160 129 master.putc(txdata[i]);
takaoha 5:42c8babe5160 130 }
takaoha 5:42c8babe5160 131 wait(waittime);
takaoha 5:42c8babe5160 132 en=0;
takaoha 5:42c8babe5160 133
takaoha 5:42c8babe5160 134 for(int i=0;i<rxlen;i++){
takaoha 5:42c8babe5160 135 rxdata[i]=master.getc()&0x1F;
takaoha 5:42c8babe5160 136 }
takaoha 5:42c8babe5160 137 printf("motor id is %d\r\n",rxdata[0]);
takaoha 5:42c8babe5160 138
takaoha 5:42c8babe5160 139 }
takaoha 5:42c8babe5160 140
takaoha 5:42c8babe5160 141 void krs::setspeed(int id){
takaoha 5:42c8babe5160 142 txlen=3;
takaoha 5:42c8babe5160 143 rxlen=3;
takaoha 5:42c8babe5160 144 unsigned char txdata[txlen];
takaoha 5:42c8babe5160 145 unsigned char rxdata[rxlen];
takaoha 5:42c8babe5160 146
takaoha 5:42c8babe5160 147 en=1;
takaoha 5:42c8babe5160 148 txdata[0]=speed_cmd+id;
takaoha 5:42c8babe5160 149 txdata[1]=speed_sc;
takaoha 5:42c8babe5160 150 txdata[2]=max_speed;
takaoha 5:42c8babe5160 151
takaoha 5:42c8babe5160 152 for(int i=0;i<txlen;i++){
takaoha 5:42c8babe5160 153 rxdata[i]=master.putc(txdata[i]);
takaoha 5:42c8babe5160 154 }
takaoha 5:42c8babe5160 155 wait(waittime);
takaoha 5:42c8babe5160 156 en=0;
takaoha 5:42c8babe5160 157
takaoha 5:42c8babe5160 158 for(int i=0;i<rxlen;i++){
takaoha 5:42c8babe5160 159 rxdata[i]=master.getc();
takaoha 5:42c8babe5160 160 }
takaoha 5:42c8babe5160 161 printf("motor %d speed is %d now\r\n",id,rxdata[2]);
takaoha 5:42c8babe5160 162 }
takaoha 5:42c8babe5160 163
takaoha 5:42c8babe5160 164 void krs::setstrech(int id,int strech){
takaoha 5:42c8babe5160 165 txlen=3;
takaoha 5:42c8babe5160 166 rxlen=3;
takaoha 5:42c8babe5160 167 unsigned char txdata[txlen];
takaoha 5:42c8babe5160 168 unsigned char rxdata[rxlen];
takaoha 5:42c8babe5160 169
takaoha 5:42c8babe5160 170 en=1;
takaoha 5:42c8babe5160 171 txdata[0]=strech_cmd+id;
takaoha 5:42c8babe5160 172 txdata[1]=strech_sc;
takaoha 5:42c8babe5160 173 if(strech==1){
takaoha 5:42c8babe5160 174 txdata[2]=weak_strech;
takaoha 5:42c8babe5160 175 }else if(strech==2){
takaoha 5:42c8babe5160 176 txdata[2]=mid_strech;
takaoha 5:42c8babe5160 177 } else{
takaoha 5:42c8babe5160 178 txdata[2]=max_strech;
takaoha 5:42c8babe5160 179 }
takaoha 6:2f9e3f56b101 180
takaoha 5:42c8babe5160 181 master.putc(txdata[0]);
takaoha 5:42c8babe5160 182 master.putc(txdata[1]);
takaoha 5:42c8babe5160 183 master.putc(txdata[2]);
takaoha 5:42c8babe5160 184
takaoha 5:42c8babe5160 185 wait(waittime);
takaoha 5:42c8babe5160 186 en=0;
takaoha 5:42c8babe5160 187
takaoha 5:42c8babe5160 188 for(int i=0;i<rxlen;i++){
takaoha 5:42c8babe5160 189 rxdata[i]=master.getc();
takaoha 5:42c8babe5160 190 }
takaoha 5:42c8babe5160 191 printf("motor %d strech is %d now\r\n",id,rxdata[2]);
takaoha 5:42c8babe5160 192 }
takaoha 5:42c8babe5160 193
takaoha 5:42c8babe5160 194 void krs::set_slavemode_off(int id){
takaoha 5:42c8babe5160 195 txlen=4;
takaoha 5:42c8babe5160 196 rxlen=2;
takaoha 5:42c8babe5160 197
takaoha 5:42c8babe5160 198 unsigned char txdata[txlen];
takaoha 5:42c8babe5160 199 unsigned char rxdata[rxlen];
takaoha 5:42c8babe5160 200
takaoha 5:42c8babe5160 201 en=1;
takaoha 5:42c8babe5160 202 txdata[0]=efform_cmd+id;
takaoha 5:42c8babe5160 203 txdata[1]=efform_sc;
takaoha 5:42c8babe5160 204 txdata[2]=slave_off_up;
takaoha 5:42c8babe5160 205 txdata[3]=slave_off_down;
takaoha 5:42c8babe5160 206
takaoha 5:42c8babe5160 207 for(int i=0;i<txlen;i++){
takaoha 5:42c8babe5160 208 master.putc(txdata[i]);
takaoha 5:42c8babe5160 209 }
takaoha 5:42c8babe5160 210 wait(waittime);
takaoha 5:42c8babe5160 211 en=0;
takaoha 5:42c8babe5160 212
takaoha 5:42c8babe5160 213 for(int i=0;i<rxlen;i++){
takaoha 5:42c8babe5160 214 rxdata[i]=master.getc();
takaoha 5:42c8babe5160 215 }
takaoha 5:42c8babe5160 216 if(rxdata[0]==id && rxdata[1]==efform_sc){
takaoha 5:42c8babe5160 217 printf("motor is not slave mode now\r\n");
takaoha 5:42c8babe5160 218 }else {
takaoha 5:42c8babe5160 219 printf("%d,%d\r\n",rxdata[0],rxdata[1]);
takaoha 5:42c8babe5160 220 }
takaoha 5:42c8babe5160 221 }
takaoha 5:42c8babe5160 222
takaoha 5:42c8babe5160 223 void krs::set_slavemode(int id){
takaoha 5:42c8babe5160 224 txlen=4;
takaoha 5:42c8babe5160 225 rxlen=2;
takaoha 5:42c8babe5160 226
takaoha 5:42c8babe5160 227 unsigned char txdata[txlen];
takaoha 5:42c8babe5160 228 unsigned char rxdata[rxlen];
takaoha 5:42c8babe5160 229
takaoha 5:42c8babe5160 230 en=1;
takaoha 5:42c8babe5160 231 txdata[0]=efform_cmd+id;
takaoha 5:42c8babe5160 232 txdata[1]=efform_sc;
takaoha 5:42c8babe5160 233 txdata[2]=slave_on_up;
takaoha 5:42c8babe5160 234 txdata[3]=slave_on_down;
takaoha 5:42c8babe5160 235
takaoha 5:42c8babe5160 236 for(int i=0;i<txlen;i++){
takaoha 5:42c8babe5160 237 master.putc(txdata[i]);
takaoha 5:42c8babe5160 238 }
takaoha 5:42c8babe5160 239
takaoha 5:42c8babe5160 240 wait(waittime);
takaoha 5:42c8babe5160 241 en=0;
takaoha 5:42c8babe5160 242
takaoha 5:42c8babe5160 243 for(int i=0;i<rxlen;i++){
takaoha 5:42c8babe5160 244 rxdata[i]=master.getc();
takaoha 5:42c8babe5160 245 }
takaoha 5:42c8babe5160 246 if(rxdata[0]==id && rxdata[1]==efform_sc){
takaoha 5:42c8babe5160 247 printf("motor is slave mode now\r\n");
takaoha 5:42c8babe5160 248 }else {
takaoha 5:42c8babe5160 249 printf("%d,%d\r\n",rxdata[0],rxdata[1]);
takaoha 5:42c8babe5160 250 }
takaoha 5:42c8babe5160 251 }