Chawisorn samrit / Mbed 2 deprecated M89_ST_Dynamixel

gogo

Dependencies:   mbed AX12

main.cpp@1:e9e4edd823e5, 2019-04-24 (annotated)

Committer:
csggreen
Date:
Wed Apr 24 05:26:53 2019 +0000
Revision:
1:e9e4edd823e5
Parent:
0:f4444dfcd74c 
g

Who changed what in which revision?

UserRevisionLine numberNew contents of line
csggreen 1:e9e4edd823e5 1
moove1334 0:f4444dfcd74c 2 #include "mbed.h"
moove1334 0:f4444dfcd74c 3 #include "AX12.h"
csggreen 1:e9e4edd823e5 4 #include <math.h>
csggreen 1:e9e4edd823e5 5 Serial device(D1, D0);
csggreen 1:e9e4edd823e5 6 DigitalOut led1(LED2);
moove1334 0:f4444dfcd74c 7
moove1334 0:f4444dfcd74c 8 AX12 ax12(PA_9,PA_10,0x01,1000000);
moove1334 0:f4444dfcd74c 9 DigitalOut TxEn (D4);
moove1334 0:f4444dfcd74c 10
csggreen 1:e9e4edd823e5 11 void Rx_interrupt();
csggreen 1:e9e4edd823e5 12 void SetSerial(int c);
csggreen 1:e9e4edd823e5 13
csggreen 1:e9e4edd823e5 14 int state_of_ST1 = 0;
csggreen 1:e9e4edd823e5 15 int state_of_ST2 = 0;
csggreen 1:e9e4edd823e5 16 int state_of_ST3 = 0;
csggreen 1:e9e4edd823e5 17
csggreen 1:e9e4edd823e5 18 float u_j1=0;
csggreen 1:e9e4edd823e5 19 float u_j2=0;
csggreen 1:e9e4edd823e5 20 float u_j3=0;
csggreen 1:e9e4edd823e5 21
csggreen 1:e9e4edd823e5 22 void drive_motor_1();
csggreen 1:e9e4edd823e5 23 void drive_motor_2();
csggreen 1:e9e4edd823e5 24 void drive_motor_3();
csggreen 1:e9e4edd823e5 25 void calculation();
csggreen 1:e9e4edd823e5 26
csggreen 1:e9e4edd823e5 27 //GREEN-------------------------------------------------------------
csggreen 1:e9e4edd823e5 28 //motor set 1
csggreen 1:e9e4edd823e5 29 DigitalOut ENA_1(PB_14);
csggreen 1:e9e4edd823e5 30 DigitalOut DIR_1(PC_4);
csggreen 1:e9e4edd823e5 31 DigitalOut PUL_1(PB_13);
csggreen 1:e9e4edd823e5 32
csggreen 1:e9e4edd823e5 33 //motor set 2
csggreen 1:e9e4edd823e5 34 DigitalOut ENA_2(PB_2);
csggreen 1:e9e4edd823e5 35 DigitalOut DIR_2(PB_15);
csggreen 1:e9e4edd823e5 36 DigitalOut PUL_2(PB_1);
csggreen 1:e9e4edd823e5 37
csggreen 1:e9e4edd823e5 38 //motor set 3
csggreen 1:e9e4edd823e5 39 DigitalOut ENA_3(PC_8);
csggreen 1:e9e4edd823e5 40 DigitalOut DIR_3(PC_5);
csggreen 1:e9e4edd823e5 41 DigitalOut PUL_3(PC_6);
csggreen 1:e9e4edd823e5 42
csggreen 1:e9e4edd823e5 43 //Buzzer
csggreen 1:e9e4edd823e5 44 DigitalOut Buzzer(PA_12);
csggreen 1:e9e4edd823e5 45
csggreen 1:e9e4edd823e5 46 //Vacum
csggreen 1:e9e4edd823e5 47 DigitalOut VACUM(PB_12);
csggreen 1:e9e4edd823e5 48
csggreen 1:e9e4edd823e5 49 //Servo
csggreen 1:e9e4edd823e5 50
csggreen 1:e9e4edd823e5 51 //Limit Switch
csggreen 1:e9e4edd823e5 52 DigitalIn LSwitch_1(PC_0);
csggreen 1:e9e4edd823e5 53 DigitalIn LSwitch_2(PC_1);
csggreen 1:e9e4edd823e5 54 DigitalIn LSwitch_3(PB_0);
csggreen 1:e9e4edd823e5 55 DigitalIn Home_Switch(PA_1);
csggreen 1:e9e4edd823e5 56
csggreen 1:e9e4edd823e5 57 // SET VELOCITY
csggreen 1:e9e4edd823e5 58 float VelocityST_1 = 30; // rpm
csggreen 1:e9e4edd823e5 59 float VelocityST_2 = 100; // rpm
csggreen 1:e9e4edd823e5 60 float VelocityST_3 = 100; // rpm
csggreen 1:e9e4edd823e5 61
csggreen 1:e9e4edd823e5 62 void open_vacum(){
csggreen 1:e9e4edd823e5 63 VACUM = 1;
csggreen 1:e9e4edd823e5 64 }
csggreen 1:e9e4edd823e5 65 void close_vacum(){
csggreen 1:e9e4edd823e5 66 VACUM = 0;
csggreen 1:e9e4edd823e5 67 }
csggreen 1:e9e4edd823e5 68 void open_buzzer(){
csggreen 1:e9e4edd823e5 69 Buzzer = 1;
csggreen 1:e9e4edd823e5 70 }
csggreen 1:e9e4edd823e5 71 void close_buzzer(){
csggreen 1:e9e4edd823e5 72 Buzzer = 0;
csggreen 1:e9e4edd823e5 73 }
csggreen 1:e9e4edd823e5 74 void set_home(){
csggreen 1:e9e4edd823e5 75 if (LSwitch_2.read() != 0){
csggreen 1:e9e4edd823e5 76 state_of_ST2=2;
csggreen 1:e9e4edd823e5 77 u_j2=180;
csggreen 1:e9e4edd823e5 78 drive_motor_2();
csggreen 1:e9e4edd823e5 79 }
csggreen 1:e9e4edd823e5 80 if (LSwitch_1.read() != 0){
csggreen 1:e9e4edd823e5 81 state_of_ST1 =2;
csggreen 1:e9e4edd823e5 82 u_j1=180;
csggreen 1:e9e4edd823e5 83 drive_motor_1();
csggreen 1:e9e4edd823e5 84 state_of_ST1 =1;
csggreen 1:e9e4edd823e5 85 u_j1=45;
csggreen 1:e9e4edd823e5 86 drive_motor_1();
csggreen 1:e9e4edd823e5 87 }
csggreen 1:e9e4edd823e5 88 if (LSwitch_3.read() != 0){
csggreen 1:e9e4edd823e5 89 state_of_ST3=2;
csggreen 1:e9e4edd823e5 90 u_j3=180;
csggreen 1:e9e4edd823e5 91 drive_motor_3();
csggreen 1:e9e4edd823e5 92 state_of_ST3=1;
csggreen 1:e9e4edd823e5 93 u_j3=70;
csggreen 1:e9e4edd823e5 94 drive_motor_3();
csggreen 1:e9e4edd823e5 95 }
csggreen 1:e9e4edd823e5 96 ax12.SetGoal(90, 1);
csggreen 1:e9e4edd823e5 97 }
csggreen 1:e9e4edd823e5 98 //GREEN-------------------------------------------------------------
csggreen 1:e9e4edd823e5 99
csggreen 1:e9e4edd823e5 100 int data_size = 16;
csggreen 1:e9e4edd823e5 101 char data[16] = {};
csggreen 1:e9e4edd823e5 102 char package = 0;
csggreen 1:e9e4edd823e5 103 char num_data = 0;
csggreen 1:e9e4edd823e5 104
csggreen 1:e9e4edd823e5 105 float q[4] = {};
csggreen 1:e9e4edd823e5 106 void ConvertData2q()
csggreen 1:e9e4edd823e5 107 {
csggreen 1:e9e4edd823e5 108 char q_mode[4] = {data[3], data[6], data[9], data[12]};
csggreen 1:e9e4edd823e5 109 char q_int[4] = {data[4], data[7], data[10], data[13]};
csggreen 1:e9e4edd823e5 110 char q_dec[4] = {data[5], data[8], data[11], data[14]};
csggreen 1:e9e4edd823e5 111 for(int i=0;i<4;i++)
csggreen 1:e9e4edd823e5 112 {
csggreen 1:e9e4edd823e5 113 if(q_mode[i] == 1)
csggreen 1:e9e4edd823e5 114 {
csggreen 1:e9e4edd823e5 115 q[i] = q_int[i]+q_dec[i]/100;
csggreen 1:e9e4edd823e5 116 }
csggreen 1:e9e4edd823e5 117 else if(q_mode[i] == 2)
csggreen 1:e9e4edd823e5 118 {
csggreen 1:e9e4edd823e5 119 q[i] = (-1)*(q_int[i]+q_dec[i]/100);
csggreen 1:e9e4edd823e5 120 }
csggreen 1:e9e4edd823e5 121 }
csggreen 1:e9e4edd823e5 122 }
csggreen 1:e9e4edd823e5 123
csggreen 1:e9e4edd823e5 124 int main()
csggreen 1:e9e4edd823e5 125 {
csggreen 1:e9e4edd823e5 126 device.baud(115200);
csggreen 1:e9e4edd823e5 127 device.attach(&Rx_interrupt);
csggreen 1:e9e4edd823e5 128 close_buzzer();
csggreen 1:e9e4edd823e5 129 close_vacum();
csggreen 1:e9e4edd823e5 130 //set_home();
csggreen 1:e9e4edd823e5 131 while(1)
csggreen 1:e9e4edd823e5 132 {
csggreen 1:e9e4edd823e5 133 if (package == 1)
csggreen 1:e9e4edd823e5 134 {
csggreen 1:e9e4edd823e5 135 package = 0;
csggreen 1:e9e4edd823e5 136 if(data[2]==1){
csggreen 1:e9e4edd823e5 137 open_buzzer();
csggreen 1:e9e4edd823e5 138 open_vacum();
csggreen 1:e9e4edd823e5 139 set_home();
csggreen 1:e9e4edd823e5 140 data[2]=0;
csggreen 1:e9e4edd823e5 141 }
csggreen 1:e9e4edd823e5 142 if(data[2]==2){
csggreen 1:e9e4edd823e5 143 ConvertData2q();
csggreen 1:e9e4edd823e5 144 state_of_ST1=data[3];
csggreen 1:e9e4edd823e5 145 u_j1=abs(q[0]);
csggreen 1:e9e4edd823e5 146 state_of_ST2=data[6];
csggreen 1:e9e4edd823e5 147 u_j2=abs(q[1]);
csggreen 1:e9e4edd823e5 148 state_of_ST3=data[9];
csggreen 1:e9e4edd823e5 149 u_j3=abs(q[2]);
csggreen 1:e9e4edd823e5 150 drive_motor_1();
csggreen 1:e9e4edd823e5 151 drive_motor_2();
csggreen 1:e9e4edd823e5 152 drive_motor_3();
csggreen 1:e9e4edd823e5 153 TxEn = 1;
csggreen 1:e9e4edd823e5 154 ax12.SetCRSpeed(0.1);
csggreen 1:e9e4edd823e5 155 ax12.SetGoal(abs(q[3]), 1);
csggreen 1:e9e4edd823e5 156 if(abs(q[3])==180){
csggreen 1:e9e4edd823e5 157 open_vacum();
csggreen 1:e9e4edd823e5 158 }
csggreen 1:e9e4edd823e5 159 if(abs(q[3])==90){
csggreen 1:e9e4edd823e5 160 wait(3);
csggreen 1:e9e4edd823e5 161 close_vacum();
csggreen 1:e9e4edd823e5 162 }
csggreen 1:e9e4edd823e5 163 }
csggreen 1:e9e4edd823e5 164 }
csggreen 1:e9e4edd823e5 165 }
csggreen 1:e9e4edd823e5 166 }
csggreen 1:e9e4edd823e5 167
csggreen 1:e9e4edd823e5 168 void Rx_interrupt()
csggreen 1:e9e4edd823e5 169 {
csggreen 1:e9e4edd823e5 170 char c = device.getc();
csggreen 1:e9e4edd823e5 171 int i = (int)c;
csggreen 1:e9e4edd823e5 172 SetSerial(i);
csggreen 1:e9e4edd823e5 173 }
csggreen 1:e9e4edd823e5 174 void SetSerial(int c)
csggreen 1:e9e4edd823e5 175 {
csggreen 1:e9e4edd823e5 176 if (num_data < 2){
csggreen 1:e9e4edd823e5 177 if (c == 255){
csggreen 1:e9e4edd823e5 178 data[num_data] = c;
csggreen 1:e9e4edd823e5 179 num_data++;
csggreen 1:e9e4edd823e5 180 }else{
csggreen 1:e9e4edd823e5 181 data[num_data] = c;
csggreen 1:e9e4edd823e5 182 num_data = 0;
csggreen 1:e9e4edd823e5 183 }
csggreen 1:e9e4edd823e5 184 }
csggreen 1:e9e4edd823e5 185 else if (num_data < data_size){
csggreen 1:e9e4edd823e5 186 data[num_data] = c;
csggreen 1:e9e4edd823e5 187 num_data++;
csggreen 1:e9e4edd823e5 188 if (num_data >= data_size){
csggreen 1:e9e4edd823e5 189 if (data[data_size-1]==255){
csggreen 1:e9e4edd823e5 190 num_data = 0;
csggreen 1:e9e4edd823e5 191 package = 1;
csggreen 1:e9e4edd823e5 192 }
csggreen 1:e9e4edd823e5 193 else num_data = 0;
csggreen 1:e9e4edd823e5 194 }
csggreen 1:e9e4edd823e5 195 }
csggreen 1:e9e4edd823e5 196 }
csggreen 1:e9e4edd823e5 197 void drive_motor_1(){
csggreen 1:e9e4edd823e5 198 float round_1 = u_j1 * 8000/360;//1:10 rpm x step pluse u_j1 default 60000
csggreen 1:e9e4edd823e5 199 float pluseforST_1 =(60/(VelocityST_1*800))/2;
csggreen 1:e9e4edd823e5 200 if (state_of_ST1==1){
csggreen 1:e9e4edd823e5 201 ENA_1 = 1;
csggreen 1:e9e4edd823e5 202 DIR_1 = 0;
csggreen 1:e9e4edd823e5 203 for (int i=0; i< round_1; i++){
csggreen 1:e9e4edd823e5 204 PUL_1 = 1;
csggreen 1:e9e4edd823e5 205 wait(pluseforST_1);//default 0.0005
csggreen 1:e9e4edd823e5 206 PUL_1 = 0;
csggreen 1:e9e4edd823e5 207 wait(pluseforST_1);//default 0.0005
csggreen 1:e9e4edd823e5 208 }
csggreen 1:e9e4edd823e5 209 //state_of_ST1 = 0;
csggreen 1:e9e4edd823e5 210 }
csggreen 1:e9e4edd823e5 211 if (state_of_ST1==2){
csggreen 1:e9e4edd823e5 212 ENA_1 = 1;
csggreen 1:e9e4edd823e5 213 DIR_1 = 1;
csggreen 1:e9e4edd823e5 214 for (int i=0; i< round_1; i++){
csggreen 1:e9e4edd823e5 215 PUL_1 = 1;
csggreen 1:e9e4edd823e5 216 wait(pluseforST_1);//default 0.0005
csggreen 1:e9e4edd823e5 217 PUL_1 = 0;
csggreen 1:e9e4edd823e5 218 wait(pluseforST_1);//default 0.0005
csggreen 1:e9e4edd823e5 219 if (LSwitch_1.read() == 0)
csggreen 1:e9e4edd823e5 220 {
csggreen 1:e9e4edd823e5 221 break;
csggreen 1:e9e4edd823e5 222 }
csggreen 1:e9e4edd823e5 223 }
csggreen 1:e9e4edd823e5 224 }
csggreen 1:e9e4edd823e5 225 }
csggreen 1:e9e4edd823e5 226 void drive_motor_2(){
csggreen 1:e9e4edd823e5 227 float round_2 = u_j2 * 40000/360;// 1:20rpm x step pluse u_j1 default 60000
csggreen 1:e9e4edd823e5 228 float pluseforST_2 =(60/(VelocityST_2*800))/2;
csggreen 1:e9e4edd823e5 229 if (state_of_ST2==1){
csggreen 1:e9e4edd823e5 230 ENA_2 = 1;
csggreen 1:e9e4edd823e5 231 DIR_2 = 1;
csggreen 1:e9e4edd823e5 232 for (int i=0; i< round_2; i++){
csggreen 1:e9e4edd823e5 233 PUL_2 = 1;
csggreen 1:e9e4edd823e5 234 wait(pluseforST_2);//default 0.0005
csggreen 1:e9e4edd823e5 235 PUL_2 = 0;
csggreen 1:e9e4edd823e5 236 wait(pluseforST_2);//default 0.0005
csggreen 1:e9e4edd823e5 237 }
csggreen 1:e9e4edd823e5 238 //state_of_ST1 = 0;
csggreen 1:e9e4edd823e5 239
csggreen 1:e9e4edd823e5 240 }
csggreen 1:e9e4edd823e5 241 if (state_of_ST2==2){
csggreen 1:e9e4edd823e5 242 ENA_2 = 1;
csggreen 1:e9e4edd823e5 243 DIR_2 = 0;
csggreen 1:e9e4edd823e5 244 for (int i=0; i< round_2; i++){
csggreen 1:e9e4edd823e5 245 PUL_2 = 1;
csggreen 1:e9e4edd823e5 246 wait(pluseforST_2);//default 0.0005
csggreen 1:e9e4edd823e5 247 PUL_2 = 0;
csggreen 1:e9e4edd823e5 248 wait(pluseforST_2);//default 0.0005
csggreen 1:e9e4edd823e5 249 if (LSwitch_2.read() == 0)
csggreen 1:e9e4edd823e5 250 {
csggreen 1:e9e4edd823e5 251 break;
csggreen 1:e9e4edd823e5 252 }
csggreen 1:e9e4edd823e5 253 }
csggreen 1:e9e4edd823e5 254 }
csggreen 1:e9e4edd823e5 255 }
csggreen 1:e9e4edd823e5 256
csggreen 1:e9e4edd823e5 257 void drive_motor_3(){
csggreen 1:e9e4edd823e5 258 float round_3 = u_j3 * 16000 /360;//1:40 rpm x step pluse u_j1 default 60000
csggreen 1:e9e4edd823e5 259 float pluseforST_3 =(60/(VelocityST_3*800))/2;
csggreen 1:e9e4edd823e5 260 if (state_of_ST3==1){
csggreen 1:e9e4edd823e5 261 ENA_3 = 1;
csggreen 1:e9e4edd823e5 262 DIR_3 = 0;
csggreen 1:e9e4edd823e5 263 for (int i=0; i< round_3; i++){
csggreen 1:e9e4edd823e5 264 PUL_3 = 1;
csggreen 1:e9e4edd823e5 265 wait(pluseforST_3);//default 0.0005
csggreen 1:e9e4edd823e5 266 PUL_3 = 0;
csggreen 1:e9e4edd823e5 267 wait(pluseforST_3);//default 0.0005
csggreen 1:e9e4edd823e5 268 }
csggreen 1:e9e4edd823e5 269 //state_of_ST1 = 0;
csggreen 1:e9e4edd823e5 270 }
csggreen 1:e9e4edd823e5 271 if (state_of_ST3==2){
csggreen 1:e9e4edd823e5 272 ENA_3 = 1;
csggreen 1:e9e4edd823e5 273 DIR_3 = 1;
csggreen 1:e9e4edd823e5 274 for (int i=0; i< round_3; i++){
csggreen 1:e9e4edd823e5 275 PUL_3 = 1;
csggreen 1:e9e4edd823e5 276 wait(pluseforST_3);//default 0.0005
csggreen 1:e9e4edd823e5 277 PUL_3 = 0;
csggreen 1:e9e4edd823e5 278 wait(pluseforST_3);//default 0.0005
csggreen 1:e9e4edd823e5 279 if (LSwitch_3.read() == 0)
csggreen 1:e9e4edd823e5 280 {
csggreen 1:e9e4edd823e5 281 break;
csggreen 1:e9e4edd823e5 282 }
csggreen 1:e9e4edd823e5 283 }
csggreen 1:e9e4edd823e5 284 }
csggreen 1:e9e4edd823e5 285 }