shota murakami / Mbed 2 deprecated mbed_servo_IDchange

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

main.cpp@0:c2319e0e250e, 2018-05-28 (annotated)

Committer:
huumi
Date:
Mon May 28 05:16:46 2018 +0000
Revision:
0:c2319e0e250e
????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
huumi 0:c2319e0e250e 1 #include"mbed.h"
huumi 0:c2319e0e250e 2
huumi 0:c2319e0e250e 3 Serial device(p28,p27);//tx:left rx:right
huumi 0:c2319e0e250e 4 Serial pc(USBTX, USBRX);//pcとの通信設定
huumi 0:c2319e0e250e 5 DigitalOut REDE(LED1);
huumi 0:c2319e0e250e 6
huumi 0:c2319e0e250e 7
huumi 0:c2319e0e250e 8 int data[2]={7,1};
huumi 0:c2319e0e250e 9
huumi 0:c2319e0e250e 10 //通信設定
huumi 0:c2319e0e250e 11 void init(void){
huumi 0:c2319e0e250e 12 device.baud(115200);
huumi 0:c2319e0e250e 13 pc.baud(115200);
huumi 0:c2319e0e250e 14 REDE = 0;
huumi 0:c2319e0e250e 15 }
huumi 0:c2319e0e250e 16
huumi 0:c2319e0e250e 17 //ID書き換え
huumi 0:c2319e0e250e 18 void IDchange(unsigned char ID, unsigned char data){
huumi 0:c2319e0e250e 19
huumi 0:c2319e0e250e 20 unsigned char Txdata[9]; //bytedata[9byte]
huumi 0:c2319e0e250e 21 unsigned char CheckSum = 0;
huumi 0:c2319e0e250e 22
huumi 0:c2319e0e250e 23 Txdata[0] = 0xFA; //Header
huumi 0:c2319e0e250e 24 Txdata[1] = 0xAF; //Header
huumi 0:c2319e0e250e 25 Txdata[2] = ID; //ID
huumi 0:c2319e0e250e 26 Txdata[3] = 0x00; //Flags
huumi 0:c2319e0e250e 27 Txdata[4] = 0x04; //Adress
huumi 0:c2319e0e250e 28 Txdata[5] = 0x01; //Length
huumi 0:c2319e0e250e 29 Txdata[6] = 0x01; //Count
huumi 0:c2319e0e250e 30 Txdata[7] = data; //data
huumi 0:c2319e0e250e 31 for(int i=2; i<=7; i++){
huumi 0:c2319e0e250e 32 CheckSum = CheckSum ^ Txdata[i];
huumi 0:c2319e0e250e 33 }
huumi 0:c2319e0e250e 34 Txdata[8] = CheckSum;
huumi 0:c2319e0e250e 35
huumi 0:c2319e0e250e 36 //Send Packet
huumi 0:c2319e0e250e 37 REDE = 1;
huumi 0:c2319e0e250e 38 for (int i=0; i<=8; i++){
huumi 0:c2319e0e250e 39 device.putc(Txdata[i]);
huumi 0:c2319e0e250e 40 pc.putc(Txdata[i]);
huumi 0:c2319e0e250e 41 }
huumi 0:c2319e0e250e 42 wait_us(250);
huumi 0:c2319e0e250e 43 REDE = 0;
huumi 0:c2319e0e250e 44 }
huumi 0:c2319e0e250e 45
huumi 0:c2319e0e250e 46 //データの書き込み
huumi 0:c2319e0e250e 47 void WriteROM(unsigned char ID){
huumi 0:c2319e0e250e 48
huumi 0:c2319e0e250e 49 unsigned char Txdata[8]; //bytedata[8byte]
huumi 0:c2319e0e250e 50 unsigned char CheckSum = 0;
huumi 0:c2319e0e250e 51
huumi 0:c2319e0e250e 52 Txdata[0] = 0xFA; //Header
huumi 0:c2319e0e250e 53 Txdata[1] = 0xAF; //Header
huumi 0:c2319e0e250e 54 Txdata[2] = ID; //ID
huumi 0:c2319e0e250e 55 Txdata[3] = 0x40; //Flags
huumi 0:c2319e0e250e 56 Txdata[4] = 0xFF; //Adress
huumi 0:c2319e0e250e 57 Txdata[5] = 0x00; //Length
huumi 0:c2319e0e250e 58 Txdata[6] = 0x00; //Count
huumi 0:c2319e0e250e 59 for(int i=2; i<=6; i++){
huumi 0:c2319e0e250e 60 CheckSum = CheckSum ^ Txdata[i];
huumi 0:c2319e0e250e 61 }
huumi 0:c2319e0e250e 62 Txdata[7] = CheckSum;
huumi 0:c2319e0e250e 63
huumi 0:c2319e0e250e 64 //Send Packet
huumi 0:c2319e0e250e 65 REDE = 1;
huumi 0:c2319e0e250e 66 for (int i=0; i<=7; i++){
huumi 0:c2319e0e250e 67 device.putc(Txdata[i]);
huumi 0:c2319e0e250e 68 pc.putc(Txdata[i]);
huumi 0:c2319e0e250e 69 }
huumi 0:c2319e0e250e 70 wait_us(250);
huumi 0:c2319e0e250e 71 REDE = 0;
huumi 0:c2319e0e250e 72 }
huumi 0:c2319e0e250e 73 //ＰＣから割り込み時のデータ受け取り
huumi 0:c2319e0e250e 74 void getdata(){
huumi 0:c2319e0e250e 75 for(int i=0;i<=1;i++){
huumi 0:c2319e0e250e 76 data[i] = pc.getc();
huumi 0:c2319e0e250e 77 }
huumi 0:c2319e0e250e 78
huumi 0:c2319e0e250e 79 IDchange(data[0],data[1]);
huumi 0:c2319e0e250e 80 WriteROM(data[1]);
huumi 0:c2319e0e250e 81 }
huumi 0:c2319e0e250e 82
huumi 0:c2319e0e250e 83 //トルク出力
huumi 0:c2319e0e250e 84 void torque(unsigned char ID, unsigned char data){
huumi 0:c2319e0e250e 85
huumi 0:c2319e0e250e 86 unsigned char TxData[9]; //bytedata[9byte]
huumi 0:c2319e0e250e 87 unsigned char CheckSum = 0; //CheckSum cakcukation
huumi 0:c2319e0e250e 88
huumi 0:c2319e0e250e 89 TxData[0] = 0xFA; //Header
huumi 0:c2319e0e250e 90 TxData[1] = 0xAF; //Header
huumi 0:c2319e0e250e 91 TxData[2] = ID; //ID
huumi 0:c2319e0e250e 92 TxData[3] = 0x00; //Flags
huumi 0:c2319e0e250e 93 TxData[4] = 0x24; //Adress
huumi 0:c2319e0e250e 94 TxData[5] = 0x01; //Length
huumi 0:c2319e0e250e 95 TxData[6] = 0x01; //Count
huumi 0:c2319e0e250e 96 TxData[7] = data; //data
huumi 0:c2319e0e250e 97
huumi 0:c2319e0e250e 98 //CheckSum calculation
huumi 0:c2319e0e250e 99 for(int i=2; i<=7; i++){
huumi 0:c2319e0e250e 100 CheckSum = CheckSum ^ TxData[i];
huumi 0:c2319e0e250e 101 }
huumi 0:c2319e0e250e 102 TxData[8] = CheckSum;
huumi 0:c2319e0e250e 103
huumi 0:c2319e0e250e 104 //Send Packet
huumi 0:c2319e0e250e 105 REDE = 1;
huumi 0:c2319e0e250e 106 for (int i=0; i<=8; i++){
huumi 0:c2319e0e250e 107 device.putc(TxData[i]);
huumi 0:c2319e0e250e 108 // pc.putc(TxData[i]);
huumi 0:c2319e0e250e 109 }
huumi 0:c2319e0e250e 110 wait_us(250);
huumi 0:c2319e0e250e 111 REDE = 0;
huumi 0:c2319e0e250e 112 }
huumi 0:c2319e0e250e 113
huumi 0:c2319e0e250e 114 //ゴールポジション
huumi 0:c2319e0e250e 115 void GoalPosition (unsigned char ID, int data){
huumi 0:c2319e0e250e 116
huumi 0:c2319e0e250e 117 unsigned char TxData[10];//10bytedata
huumi 0:c2319e0e250e 118 unsigned char CheckSum = 0;
huumi 0:c2319e0e250e 119
huumi 0:c2319e0e250e 120 TxData[0] = 0xFA; //Header
huumi 0:c2319e0e250e 121 TxData[1] = 0xAF; //Header
huumi 0:c2319e0e250e 122 TxData[2] = ID; //ID
huumi 0:c2319e0e250e 123 TxData[3] = 0x00; //Flags
huumi 0:c2319e0e250e 124 TxData[4] = 0x1E; //Adress
huumi 0:c2319e0e250e 125 TxData[5] = 0x02; //Length
huumi 0:c2319e0e250e 126 TxData[6] = 0x01; //Count
huumi 0:c2319e0e250e 127
huumi 0:c2319e0e250e 128 TxData[7] = (unsigned char)0x00FF & data;
huumi 0:c2319e0e250e 129 TxData[8] = (unsigned char)0x00FF & (data >> 8);
huumi 0:c2319e0e250e 130
huumi 0:c2319e0e250e 131 //CheckSum calculation
huumi 0:c2319e0e250e 132 for(int i=2; i<=8; i++){
huumi 0:c2319e0e250e 133 CheckSum = CheckSum ^ TxData[i];
huumi 0:c2319e0e250e 134 }
huumi 0:c2319e0e250e 135 TxData[9] = CheckSum;
huumi 0:c2319e0e250e 136
huumi 0:c2319e0e250e 137 //Send Packet
huumi 0:c2319e0e250e 138 REDE = 1;
huumi 0:c2319e0e250e 139 for (int i=0; i<=9; i++){
huumi 0:c2319e0e250e 140 device.putc(TxData[i]);
huumi 0:c2319e0e250e 141 pc.putc(TxData[i]);
huumi 0:c2319e0e250e 142 }
huumi 0:c2319e0e250e 143 wait_us(250);
huumi 0:c2319e0e250e 144 REDE = 0;
huumi 0:c2319e0e250e 145 }
huumi 0:c2319e0e250e 146
huumi 0:c2319e0e250e 147 int main(){
huumi 0:c2319e0e250e 148 init();
huumi 0:c2319e0e250e 149 // pc.attach(getdata, Serial::RxIrq);
huumi 0:c2319e0e250e 150 while(1){REDE = 1;
huumi 0:c2319e0e250e 151 IDchange(data[0],data[1]);
huumi 0:c2319e0e250e 152 WriteROM(data[1]);
huumi 0:c2319e0e250e 153 torque(data[1],0x01);
huumi 0:c2319e0e250e 154 wait(0.5);
huumi 0:c2319e0e250e 155 GoalPosition(data[1],1500);
huumi 0:c2319e0e250e 156 wait(2);
huumi 0:c2319e0e250e 157 GoalPosition(data[1],-15000);
huumi 0:c2319e0e250e 158 wait(2);
huumi 0:c2319e0e250e 159 }
huumi 0:c2319e0e250e 160 }