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@4:b4c8d6671241, 2019-12-29 (annotated)

Committer:
takaoha
Date:
Sun Dec 29 16:36:37 2019 +0000
Revision:
4:b4c8d6671241
Parent:
3:e356b3e7ecfd
Child:
5:42c8babe5160
krs

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 3:e356b3e7ecfd 3
takaoha 3:e356b3e7ecfd 4 krs::krs(PinName tx, PinName rx, PinName io, int baud) : master(tx,rx),en(io)
takaoha 3:e356b3e7ecfd 5 {
takaoha 3:e356b3e7ecfd 6 master.baud(baud);
takaoha 3:e356b3e7ecfd 7 if(baud==112500){
takaoha 3:e356b3e7ecfd 8 waittime=0.0002;
takaoha 3:e356b3e7ecfd 9 }else if(baud==1250000){
takaoha 3:e356b3e7ecfd 10 waittime=0.00002;
takaoha 3:e356b3e7ecfd 11 }
takaoha 3:e356b3e7ecfd 12 master.format(8, Serial::Even, 1);
takaoha 3:e356b3e7ecfd 13 en.output();
takaoha 3:e356b3e7ecfd 14 }
takaoha 3:e356b3e7ecfd 15
takaoha 3:e356b3e7ecfd 16 bool krs::setpos(int deg,int id){
takaoha 3:e356b3e7ecfd 17 txlen=3;
takaoha 3:e356b3e7ecfd 18 rxlen=3;
takaoha 3:e356b3e7ecfd 19 int txdata[txlen];
takaoha 3:e356b3e7ecfd 20 int rxdata[rxlen];
takaoha 3:e356b3e7ecfd 21 en=1;
takaoha 3:e356b3e7ecfd 22
takaoha 3:e356b3e7ecfd 23 if(deg>135 || deg<-135){
takaoha 3:e356b3e7ecfd 24 printf("failed\r\n");
takaoha 3:e356b3e7ecfd 25 return 0;
takaoha 3:e356b3e7ecfd 26 }
takaoha 3:e356b3e7ecfd 27 int pos=deg*29.633;
takaoha 3:e356b3e7ecfd 28 pos=pos+7500;
takaoha 3:e356b3e7ecfd 29
takaoha 3:e356b3e7ecfd 30 txdata[0]=0x80+id;
takaoha 3:e356b3e7ecfd 31 txdata[1]=((pos >> 7) & 0x007F);
takaoha 3:e356b3e7ecfd 32 txdata[2]=(pos & 0x007F);
takaoha 3:e356b3e7ecfd 33
takaoha 3:e356b3e7ecfd 34 for(int i=0;i<txlen;i++){
takaoha 3:e356b3e7ecfd 35 master.putc(txdata[i]);
takaoha 3:e356b3e7ecfd 36 }
takaoha 3:e356b3e7ecfd 37 wait(waittime);
takaoha 3:e356b3e7ecfd 38 //wait(0.00002);//1250000bps用
takaoha 3:e356b3e7ecfd 39 //wait(0.0002);
takaoha 4:b4c8d6671241 40 /*
takaoha 3:e356b3e7ecfd 41 en=0;
takaoha 3:e356b3e7ecfd 42 for(int i=0;i<rxlen;i++){
takaoha 3:e356b3e7ecfd 43 rxdata[i]=master.getc();
takaoha 3:e356b3e7ecfd 44 }
takaoha 3:e356b3e7ecfd 45
takaoha 3:e356b3e7ecfd 46 int degnow;
takaoha 3:e356b3e7ecfd 47 degnow=((rxdata[1] << 7) & 0x3F80) + (rxdata[2] & 0x007F);
takaoha 3:e356b3e7ecfd 48 degnow=(degnow-7500)/29.6;
takaoha 3:e356b3e7ecfd 49
takaoha 3:e356b3e7ecfd 50 /*pc.printf("%d\r\n",rxdata[0]);
takaoha 3:e356b3e7ecfd 51 pc.printf("%d\r\n",rxdata[1]);
takaoha 4:b4c8d6671241 52 pc.printf("%d\r\n",rxdata[2]);
takaoha 4:b4c8d6671241 53 printf("角度%d\r\n",degnow);*/
takaoha 3:e356b3e7ecfd 54 }
takaoha 4:b4c8d6671241 55
takaoha 3:e356b3e7ecfd 56 void krs::getpos(int id){
takaoha 4:b4c8d6671241 57 en=1;
takaoha 3:e356b3e7ecfd 58 txlen=2;
takaoha 3:e356b3e7ecfd 59 rxlen=4;
takaoha 3:e356b3e7ecfd 60 unsigned char txdata[txlen];
takaoha 3:e356b3e7ecfd 61 unsigned char rxdata[rxlen];
takaoha 3:e356b3e7ecfd 62 int degnow;
takaoha 3:e356b3e7ecfd 63
takaoha 3:e356b3e7ecfd 64 txdata[0]=0xA0+id;
takaoha 3:e356b3e7ecfd 65 txdata[1]=0x05;
takaoha 3:e356b3e7ecfd 66
takaoha 3:e356b3e7ecfd 67
takaoha 4:b4c8d6671241 68 for(int i=0;i<txlen;i++){
takaoha 3:e356b3e7ecfd 69 master.putc(txdata[i]);
takaoha 3:e356b3e7ecfd 70 }
takaoha 3:e356b3e7ecfd 71 wait(waittime);
takaoha 4:b4c8d6671241 72 //wait(0.00002);
takaoha 3:e356b3e7ecfd 73 en=0;
takaoha 3:e356b3e7ecfd 74
takaoha 4:b4c8d6671241 75 for(int i=0;i<rxlen;i++){
takaoha 3:e356b3e7ecfd 76 rxdata[i]=master.getc();
takaoha 3:e356b3e7ecfd 77 }
takaoha 3:e356b3e7ecfd 78
takaoha 3:e356b3e7ecfd 79 printf("%d\r\n",rxdata[0]);
takaoha 3:e356b3e7ecfd 80 degnow=((rxdata[2] << 7) & 0x3F80) + (rxdata[3] & 0x007F);
takaoha 3:e356b3e7ecfd 81 degnow=(degnow-7500)/29.6;
takaoha 3:e356b3e7ecfd 82 printf("%d\r\n",degnow);
takaoha 3:e356b3e7ecfd 83
takaoha 3:e356b3e7ecfd 84 }
takaoha 4:b4c8d6671241 85
takaoha 3:e356b3e7ecfd 86