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@2:33681dfc2aa5, 2019-12-24 (annotated)

Committer:
takaoha
Date:
Tue Dec 24 07:15:01 2019 +0000
Revision:
2:33681dfc2aa5
Parent:
1:fa3a640a55e0
Child:
3:e356b3e7ecfd
krs3304;

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