Abe Takumi / Mbed 2 deprecated RS405CB_test

meister2013 control test program

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }