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