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@1:fa3a640a55e0, 2019-12-23 (annotated)

Committer:
takaoha
Date:
Mon Dec 23 15:36:06 2019 +0000
Revision:
1:fa3a640a55e0
Parent:
0:651a188c5dc3
Child:
2:33681dfc2aa5
kondo servo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Soutajin 0:651a188c5dc3 1 #include "mbed.h"
takaoha 1:fa3a640a55e0 2 #include "math.h"
takaoha 1:fa3a640a55e0 3
takaoha 1:fa3a640a55e0 4
takaoha 1:fa3a640a55e0 5 DigitalInOut name(PC_7);//EN_IN(HIGHでICS機器に送信するモード、LOWでICS機器から受信するモードにするピン)と繋いでいる
takaoha 1:fa3a640a55e0 6 Serial master(PA_9, PA_10);//シリアル通信を担当するピン
takaoha 1:fa3a640a55e0 7
takaoha 1:fa3a640a55e0 8 Serial pc(USBTX,USBRX);
takaoha 1:fa3a640a55e0 9 bool setpos(int deg,int id);
takaoha 1:fa3a640a55e0 10 void getID();
takaoha 1:fa3a640a55e0 11
takaoha 1:fa3a640a55e0 12 //int getpos(int id);
takaoha 1:fa3a640a55e0 13 int txlen=3;
takaoha 1:fa3a640a55e0 14 int rxlen=3;
takaoha 1:fa3a640a55e0 15 int _id;
takaoha 1:fa3a640a55e0 16
Soutajin 0:651a188c5dc3 17 int main() {
takaoha 1:fa3a640a55e0 18 pc.printf("hello\r\n");
takaoha 1:fa3a640a55e0 19 master.baud(1250000);//クロックレートの設定
Soutajin 0:651a188c5dc3 20 master.format(8, Serial::Even, 1);//通信方式の設定
Soutajin 0:651a188c5dc3 21 name.output();//p8を出力モードに
takaoha 1:fa3a640a55e0 22
takaoha 1:fa3a640a55e0 23 int deg=0;
takaoha 1:fa3a640a55e0 24 char degpart[3];
takaoha 1:fa3a640a55e0 25 int id;
takaoha 1:fa3a640a55e0 26
takaoha 1:fa3a640a55e0 27 while(1){
takaoha 1:fa3a640a55e0 28 pc.printf("degree\r\n");
takaoha 1:fa3a640a55e0 29 for(int i=0;i<3;i++){
takaoha 1:fa3a640a55e0 30 degpart[i]=pc.getc()-48;
takaoha 1:fa3a640a55e0 31 printf("%d",degpart[i]);
takaoha 1:fa3a640a55e0 32 }
takaoha 1:fa3a640a55e0 33 printf("\r\n");
takaoha 1:fa3a640a55e0 34 deg=100*degpart[0]+10*degpart[1]+degpart[2];
takaoha 1:fa3a640a55e0 35 //pc.scanf("%d\r\n",deg);
takaoha 1:fa3a640a55e0 36 pc.printf("%d\r\n",deg);
takaoha 1:fa3a640a55e0 37 pc.printf("id\r\n");
takaoha 1:fa3a640a55e0 38 id=pc.getc()-48;
takaoha 1:fa3a640a55e0 39 //id+=48;
takaoha 1:fa3a640a55e0 40 pc.printf("%d\r\n",id);
takaoha 1:fa3a640a55e0 41 setpos(deg,id);
takaoha 1:fa3a640a55e0 42 //getID();
takaoha 1:fa3a640a55e0 43 }
takaoha 1:fa3a640a55e0 44 }
takaoha 1:fa3a640a55e0 45
takaoha 1:fa3a640a55e0 46 bool setpos(int deg,int id){
takaoha 1:fa3a640a55e0 47 name=1;
takaoha 1:fa3a640a55e0 48 int txdata[txlen];
takaoha 1:fa3a640a55e0 49 int rxdata[rxlen];
takaoha 1:fa3a640a55e0 50 if(deg>135 || deg<-135){
takaoha 1:fa3a640a55e0 51 pc.printf("failed\r\n");
takaoha 1:fa3a640a55e0 52 return 0;
takaoha 1:fa3a640a55e0 53 }
takaoha 1:fa3a640a55e0 54 int pos=deg*29.633;
takaoha 1:fa3a640a55e0 55 pos=pos+7500;
takaoha 1:fa3a640a55e0 56 //pc.printf("%d\r\n",pos);
takaoha 1:fa3a640a55e0 57 txdata[0]=0x80+id;
takaoha 1:fa3a640a55e0 58 txdata[1]=((pos >> 7) & 0x007F);
takaoha 1:fa3a640a55e0 59 txdata[2]=(pos & 0x007F);
takaoha 1:fa3a640a55e0 60
takaoha 1:fa3a640a55e0 61 for(int i=0;i<txlen;i++){
takaoha 1:fa3a640a55e0 62 master.putc(txdata[i]);
takaoha 1:fa3a640a55e0 63 }
takaoha 1:fa3a640a55e0 64 wait(0.00002);
takaoha 1:fa3a640a55e0 65
takaoha 1:fa3a640a55e0 66 name=0;
takaoha 1:fa3a640a55e0 67
takaoha 1:fa3a640a55e0 68
takaoha 1:fa3a640a55e0 69 for(int i=0;i<rxlen;i++){
takaoha 1:fa3a640a55e0 70 rxdata[i]=master.getc();
takaoha 1:fa3a640a55e0 71 //wait(0.05);
takaoha 1:fa3a640a55e0 72 //pc.printf("%d\r\n",rxdata[i]);
takaoha 1:fa3a640a55e0 73 }
takaoha 1:fa3a640a55e0 74 //wait(0.01);
takaoha 1:fa3a640a55e0 75 //name=1;
takaoha 1:fa3a640a55e0 76
takaoha 1:fa3a640a55e0 77 //if(i==txlen-1){
takaoha 1:fa3a640a55e0 78 //name=0;
takaoha 1:fa3a640a55e0 79
takaoha 1:fa3a640a55e0 80
takaoha 1:fa3a640a55e0 81 /*for(int i=0;i<rxlen;i++){
takaoha 1:fa3a640a55e0 82 //pc.printf("h\r\n");
takaoha 1:fa3a640a55e0 83
takaoha 1:fa3a640a55e0 84 //pc.printf("hello\r\n");
takaoha 1:fa3a640a55e0 85 rxdata[i]=master.getc();
takaoha 1:fa3a640a55e0 86 }*/
takaoha 1:fa3a640a55e0 87
takaoha 1:fa3a640a55e0 88 //pc.printf("OK\r\n");
takaoha 1:fa3a640a55e0 89
takaoha 1:fa3a640a55e0 90 //wait(0.05);
takaoha 1:fa3a640a55e0 91 //pc.printf("hello\r\n");
takaoha 1:fa3a640a55e0 92
takaoha 1:fa3a640a55e0 93
takaoha 1:fa3a640a55e0 94
takaoha 1:fa3a640a55e0 95 //pc.printf("hello\r\n");
takaoha 1:fa3a640a55e0 96 int degnow;
takaoha 1:fa3a640a55e0 97 degnow=((rxdata[1] << 7) & 0x3F80) + (rxdata[2] & 0x007F);
takaoha 1:fa3a640a55e0 98 degnow=(degnow-7500)/29.6;
takaoha 1:fa3a640a55e0 99 //pc.printf("%d\r\n",degnow);
takaoha 1:fa3a640a55e0 100 pc.printf("%d\r\n",rxdata[0]);
takaoha 1:fa3a640a55e0 101 pc.printf("%d\r\n",rxdata[1]);
takaoha 1:fa3a640a55e0 102 pc.printf("%d\r\n",rxdata[2]);
takaoha 1:fa3a640a55e0 103 pc.printf("角度%d\r\n",degnow);
takaoha 1:fa3a640a55e0 104 }
takaoha 1:fa3a640a55e0 105
takaoha 1:fa3a640a55e0 106 /*int getpos(int id){
takaoha 1:fa3a640a55e0 107 name=1;
takaoha 1:fa3a640a55e0 108 unsigned char txdata[txlen-1];
takaoha 1:fa3a640a55e0 109 unsigned char rxdata[rxlen];
takaoha 1:fa3a640a55e0 110 int degnow;
takaoha 1:fa3a640a55e0 111
takaoha 1:fa3a640a55e0 112 txdata[0]=0xA0+id;
takaoha 1:fa3a640a55e0 113 txdata[1]=0x05;
takaoha 1:fa3a640a55e0 114
takaoha 1:fa3a640a55e0 115 pc.printf("done\r\n");
takaoha 1:fa3a640a55e0 116 for(int i=0;i<txlen-1;i++){
takaoha 1:fa3a640a55e0 117 master.putc(txdata[i]);
takaoha 1:fa3a640a55e0 118 }
takaoha 1:fa3a640a55e0 119 name=0;
takaoha 1:fa3a640a55e0 120 wait(0.5);
takaoha 1:fa3a640a55e0 121 if(master.readable()){
takaoha 1:fa3a640a55e0 122 pc.printf("OK\r\n");
takaoha 1:fa3a640a55e0 123 }
takaoha 1:fa3a640a55e0 124 for(int i=0;i<rxlen;i++){
takaoha 1:fa3a640a55e0 125 if(master.readable()){
takaoha 1:fa3a640a55e0 126 rxdata[i]=master.getc();
takaoha 1:fa3a640a55e0 127 pc.printf("%c",rxdata[i]);
takaoha 1:fa3a640a55e0 128 }else{
takaoha 1:fa3a640a55e0 129 pc.printf("failed\r\n");
takaoha 1:fa3a640a55e0 130 }
takaoha 1:fa3a640a55e0 131 }
takaoha 1:fa3a640a55e0 132
takaoha 1:fa3a640a55e0 133
takaoha 1:fa3a640a55e0 134
takaoha 1:fa3a640a55e0 135 pc.printf("%d",rxdata[0]);
takaoha 1:fa3a640a55e0 136 degnow=((rxdata[2] << 7) & 0x3F80) + (rxdata[3] & 0x007F);
takaoha 1:fa3a640a55e0 137 //degnow=(degnow-7500)*1000/296;
takaoha 1:fa3a640a55e0 138 pc.printf("%d",degnow);
takaoha 1:fa3a640a55e0 139 return degnow;
takaoha 1:fa3a640a55e0 140 } */
takaoha 1:fa3a640a55e0 141
takaoha 1:fa3a640a55e0 142 void getID()
takaoha 1:fa3a640a55e0 143 {
takaoha 1:fa3a640a55e0 144 int id;
takaoha 1:fa3a640a55e0 145 name=1;
takaoha 1:fa3a640a55e0 146 master.putc(0xFF);
takaoha 1:fa3a640a55e0 147 master.putc(0x0);
takaoha 1:fa3a640a55e0 148 master.putc(0x0);
takaoha 1:fa3a640a55e0 149 master.putc(0x0);
takaoha 1:fa3a640a55e0 150 wait(0.01);
takaoha 1:fa3a640a55e0 151 name=0;
takaoha 1:fa3a640a55e0 152 /*for(int i=0;i<5;i++)
takaoha 1:fa3a640a55e0 153 {
takaoha 1:fa3a640a55e0 154 id = (master.getc() & 0x1f);
takaoha 1:fa3a640a55e0 155 }*/
takaoha 1:fa3a640a55e0 156 id=master.getc();
takaoha 1:fa3a640a55e0 157 printf("%d\r\n",id);
takaoha 1:fa3a640a55e0 158 wait_ms(500);
takaoha 1:fa3a640a55e0 159
takaoha 1:fa3a640a55e0 160 }