Abe Takumi / RS405cb

双葉コマンドサーボRS405cb制御用プログラム

Dependents:   EM_Mission ToyBox ohta FriedRice

RS405cb.cpp@1:4f4b999b4939, 2013-07-13 (annotated)

Committer:
YSB
Date:
Sat Jul 13 17:06:43 2013 +0000
Revision:
1:4f4b999b4939
Parent:
0:03b3a91ccd08 
Firsr Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YSB 0:03b3a91ccd08 1 #include "mbed.h"
YSB 0:03b3a91ccd08 2 #include "RS405cb.h"
YSB 0:03b3a91ccd08 3
YSB 0:03b3a91ccd08 4 RS405cb::RS405cb(PinName tx,PinName rx,PinName permit):Serial(tx,rx),_permit(permit){
YSB 0:03b3a91ccd08 5 baud(115200);
YSB 0:03b3a91ccd08 6 }
YSB 0:03b3a91ccd08 7
YSB 0:03b3a91ccd08 8 void RS405cb::Sho_Init(int center_vert,int center_hori,int pls15_vert,int pls15_hori,int mns15_vert,int mns15_hori){
YSB 1:4f4b999b4939 9 //TORQUE_ON(1);
YSB 0:03b3a91ccd08 10 TORQUE_ON(2);
YSB 0:03b3a91ccd08 11 TORQUE_ON(3);
YSB 1:4f4b999b4939 12 //TORQUE_ON(4);
YSB 0:03b3a91ccd08 13
YSB 0:03b3a91ccd08 14 center_v = center_vert;
YSB 0:03b3a91ccd08 15 center_h = center_hori;
YSB 0:03b3a91ccd08 16 pls15_v = pls15_vert;
YSB 0:03b3a91ccd08 17 pls15_h = pls15_hori;
YSB 0:03b3a91ccd08 18 mns15_v = mns15_vert;
YSB 0:03b3a91ccd08 19 mns15_h = mns15_hori;
YSB 0:03b3a91ccd08 20 }
YSB 0:03b3a91ccd08 21
YSB 0:03b3a91ccd08 22 void RS405cb::RSW1(unsigned char id,unsigned char flg1,unsigned char adr1,unsigned char len1,unsigned char cnt1,unsigned char data1){
YSB 0:03b3a91ccd08 23 unsigned char sum;
YSB 0:03b3a91ccd08 24 sum = id ^ flg1 ^ adr1 ^ len1 ^ cnt1 ^ data1;
YSB 0:03b3a91ccd08 25 _permit = 1;
YSB 0:03b3a91ccd08 26 putc(0xFA);
YSB 0:03b3a91ccd08 27 putc(0xAF);
YSB 0:03b3a91ccd08 28 putc(id);
YSB 0:03b3a91ccd08 29 putc(flg1);
YSB 0:03b3a91ccd08 30 putc(adr1);
YSB 0:03b3a91ccd08 31 putc(len1);
YSB 0:03b3a91ccd08 32 putc(cnt1);
YSB 0:03b3a91ccd08 33 putc(data1);
YSB 0:03b3a91ccd08 34 putc(sum);
YSB 0:03b3a91ccd08 35 putc(0x00);
YSB 0:03b3a91ccd08 36 //putc(0x00);
YSB 0:03b3a91ccd08 37 while(!writeable()){}
YSB 0:03b3a91ccd08 38 _permit = 0;
YSB 0:03b3a91ccd08 39 }
YSB 0:03b3a91ccd08 40
YSB 0:03b3a91ccd08 41 void RS405cb::RSW2(unsigned char id,unsigned char flg,unsigned char adr,unsigned char len,unsigned char cnt,unsigned char lowdata,unsigned char highdata){
YSB 0:03b3a91ccd08 42 unsigned char sum;
YSB 0:03b3a91ccd08 43 sum = id ^ flg ^ adr ^ len ^ cnt ^ lowdata ^ highdata;
YSB 0:03b3a91ccd08 44 _permit = 1;
YSB 0:03b3a91ccd08 45 putc(0xFA);
YSB 0:03b3a91ccd08 46 putc(0xAF);
YSB 0:03b3a91ccd08 47 putc(id);
YSB 0:03b3a91ccd08 48 putc(flg);
YSB 0:03b3a91ccd08 49 putc(adr);
YSB 0:03b3a91ccd08 50 putc(len);
YSB 0:03b3a91ccd08 51 putc(cnt);
YSB 0:03b3a91ccd08 52 putc(lowdata);
YSB 0:03b3a91ccd08 53 putc(highdata);
YSB 0:03b3a91ccd08 54 putc(sum);
YSB 0:03b3a91ccd08 55 putc(0x00);
YSB 0:03b3a91ccd08 56 //putc(0x00);
YSB 0:03b3a91ccd08 57 while(!writeable()){}
YSB 0:03b3a91ccd08 58 _permit = 0;
YSB 0:03b3a91ccd08 59 }
YSB 0:03b3a91ccd08 60
YSB 0:03b3a91ccd08 61 void RS405cb::RSW0(unsigned char id,unsigned char flg,unsigned char adr,unsigned char len,unsigned char cnt){
YSB 0:03b3a91ccd08 62 unsigned char sum;
YSB 0:03b3a91ccd08 63 sum = id ^ flg ^ adr ^ len ^ cnt;
YSB 0:03b3a91ccd08 64 _permit = 1;
YSB 0:03b3a91ccd08 65 putc(0xFA);
YSB 0:03b3a91ccd08 66 putc(0xAF);
YSB 0:03b3a91ccd08 67 putc(id);
YSB 0:03b3a91ccd08 68 putc(flg);
YSB 0:03b3a91ccd08 69 putc(adr);
YSB 0:03b3a91ccd08 70 putc(len);
YSB 0:03b3a91ccd08 71 putc(cnt);
YSB 0:03b3a91ccd08 72 putc(sum);
YSB 0:03b3a91ccd08 73 putc(0x00);
YSB 0:03b3a91ccd08 74 //putc(0x00);
YSB 0:03b3a91ccd08 75 while(!writeable()){}
YSB 0:03b3a91ccd08 76 _permit = 0;
YSB 0:03b3a91ccd08 77 }
YSB 0:03b3a91ccd08 78
YSB 0:03b3a91ccd08 79 void RS405cb::Rotate_Servo(unsigned char id,char RL,unsigned int ANGLE){
YSB 0:03b3a91ccd08 80 if(RL == RIGHT){
YSB 0:03b3a91ccd08 81 unsigned char hd1,ld1;
YSB 0:03b3a91ccd08 82 hd1 = (ANGLE & 0xFF00) >> 8;
YSB 0:03b3a91ccd08 83 ld1 = (ANGLE & 0x00FF);
YSB 0:03b3a91ccd08 84 RSW2(id,0x00,0x1E,0x02,0x01,ld1,hd1);
YSB 0:03b3a91ccd08 85 }
YSB 0:03b3a91ccd08 86 else{
YSB 0:03b3a91ccd08 87 unsigned char hd2,ld2;
YSB 0:03b3a91ccd08 88 hd2 = ((0x10000 - ANGLE) & 0xFF00) >> 8;
YSB 0:03b3a91ccd08 89 ld2 = ((0x10000 - ANGLE) & 0x00FF);
YSB 0:03b3a91ccd08 90 RSW2(id,0x00,0x1E,0x02,0x01,ld2,hd2);
YSB 0:03b3a91ccd08 91 }
YSB 0:03b3a91ccd08 92 }
YSB 0:03b3a91ccd08 93
YSB 0:03b3a91ccd08 94 void RS405cb::Rotate_Servo_Float(unsigned char id,float ANGLE){
YSB 0:03b3a91ccd08 95 unsigned char hd,ld;
YSB 0:03b3a91ccd08 96 if(ANGLE >=0){
YSB 0:03b3a91ccd08 97 hd = ((int)(ANGLE*10.0) & 0x0000FF00) >> 8;
YSB 0:03b3a91ccd08 98 ld = ((int)(ANGLE*10.0) & 0x000000FF);
YSB 0:03b3a91ccd08 99 }
YSB 0:03b3a91ccd08 100 else if(ANGLE <0){
YSB 0:03b3a91ccd08 101 hd = ((0x100000000 + (int)(ANGLE*10.0))&0x0000FF00)>>8;
YSB 0:03b3a91ccd08 102 ld = ((0x100000000 + (int)(ANGLE*10.0))&0x000000FF);
YSB 0:03b3a91ccd08 103 }
YSB 0:03b3a91ccd08 104 else{}
YSB 0:03b3a91ccd08 105 RSW2(id,0x00,0x1E,0x02,0x01,ld,hd);
YSB 0:03b3a91ccd08 106 }
YSB 0:03b3a91ccd08 107
YSB 1:4f4b999b4939 108 void RS405cb::Rotate_Servo_Float_Test(unsigned char id){
YSB 1:4f4b999b4939 109 while(1){
YSB 1:4f4b999b4939 110 TORQUE_ON(id);
YSB 1:4f4b999b4939 111 Rotate_Servo_Float(id,0.0);
YSB 1:4f4b999b4939 112 wait(1.0);
YSB 1:4f4b999b4939 113 Rotate_Servo_Float(id,50.0);
YSB 1:4f4b999b4939 114 wait(1.0);
YSB 1:4f4b999b4939 115 Rotate_Servo_Float(id,-20.0);
YSB 1:4f4b999b4939 116 wait(1.0);
YSB 1:4f4b999b4939 117 }
YSB 1:4f4b999b4939 118 }
YSB 1:4f4b999b4939 119
YSB 0:03b3a91ccd08 120 void RS405cb::Sho_Rotate_Servo(unsigned char id,int VH,float angle_of_attack){
YSB 0:03b3a91ccd08 121 int center,pls15,mns15;
YSB 0:03b3a91ccd08 122 if(VH == VERTICAL){
YSB 0:03b3a91ccd08 123 center = center_v;
YSB 0:03b3a91ccd08 124 pls15 = pls15_v;
YSB 0:03b3a91ccd08 125 mns15 = mns15_v;
YSB 0:03b3a91ccd08 126 }
YSB 0:03b3a91ccd08 127 else{
YSB 0:03b3a91ccd08 128 center = center_h;
YSB 0:03b3a91ccd08 129 pls15 = pls15_h;
YSB 0:03b3a91ccd08 130 mns15 = mns15_h;
YSB 0:03b3a91ccd08 131 }
YSB 0:03b3a91ccd08 132
YSB 0:03b3a91ccd08 133 if(angle_of_attack >= 0){Rotate_Servo_Float(id,center + (pls15 - center)*(angle_of_attack / 15.0));}
YSB 0:03b3a91ccd08 134 else{Rotate_Servo_Float(id,mns15 + (center - mns15)*((15.0 + angle_of_attack) / 15.0));}
YSB 0:03b3a91ccd08 135 }
YSB 0:03b3a91ccd08 136
YSB 0:03b3a91ccd08 137 void RS405cb::TORQUE_ON(unsigned char id){
YSB 0:03b3a91ccd08 138 RSW1(id,0x00,0x24,0x01,0x01,0x01);
YSB 0:03b3a91ccd08 139 }
YSB 0:03b3a91ccd08 140
YSB 0:03b3a91ccd08 141 void RS405cb::ID_CHANGE(unsigned char oldid,unsigned char newid){
YSB 0:03b3a91ccd08 142 RSW1(oldid,0x00,0x04,0x01,0x01,newid);
YSB 1:4f4b999b4939 143 wait(0.5);
YSB 0:03b3a91ccd08 144 RSW0(newid,0x40,0xFF,0x00,0x00);
YSB 1:4f4b999b4939 145 wait(3.0);
YSB 0:03b3a91ccd08 146 RSW0(newid,0x20,0xFF,0x00,0x00);
YSB 1:4f4b999b4939 147 while(1){
YSB 1:4f4b999b4939 148 Rotate_Servo_Float_Test(newid);
YSB 1:4f4b999b4939 149 }
YSB 0:03b3a91ccd08 150 }
YSB 0:03b3a91ccd08 151
YSB 0:03b3a91ccd08 152 void RS405cb::DECIDE_LIMIT_ANGLE(unsigned char id,unsigned int CWlimit,unsigned int CCWlimit){
YSB 0:03b3a91ccd08 153 RSW2(id,0x00,0x08,0x02,0x01,(unsigned char)(CWlimit & 0x00FF),(unsigned char)((CWlimit & 0xFF00)>>8));
YSB 0:03b3a91ccd08 154 RSW2(id,0x00,0x08,0x02,0x01,(unsigned char)((0x10000 - CCWlimit) & 0x00FF),(unsigned char)(((0x10000-CCWlimit) & 0xFF00)>>8));
YSB 0:03b3a91ccd08 155 RSW0(id,0x40,0xFF,0x00,0x00);
YSB 0:03b3a91ccd08 156 wait(2.0);
YSB 0:03b3a91ccd08 157 RSW0(id,0x20,0xFF,0x00,0x00);
YSB 0:03b3a91ccd08 158 }
YSB 0:03b3a91ccd08 159
YSB 0:03b3a91ccd08 160 void RS405cb::REQUIRE_RETURN_PACKET(unsigned char id){
YSB 0:03b3a91ccd08 161 //RSW1(id,0x09,0x24,0x01,0x01,0x01);
YSB 0:03b3a91ccd08 162 RSW0(id,0x09,0x00,0x00,0x01);
YSB 0:03b3a91ccd08 163 }