Chawisorn samrit
/
M89_ST_Dynamixel
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.
Revision 1:e9e4edd823e5, committed 2019-04-24
- Comitter:
- csggreen
- Date:
- Wed Apr 24 05:26:53 2019 +0000
- Parent:
- 0:f4444dfcd74c
- Commit message:
- g
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Apr 22 02:30:12 2019 +0000
+++ b/main.cpp Wed Apr 24 05:26:53 2019 +0000
@@ -1,14 +1,285 @@
+
#include "mbed.h"
#include "AX12.h"
+#include <math.h>
+Serial device(D1, D0);
+DigitalOut led1(LED2);
-Serial pc(D1,D0);
AX12 ax12(PA_9,PA_10,0x01,1000000);
-
DigitalOut TxEn (D4);
-int main() {
- TxEn = 1;
- ax12.SetCRSpeed(0.1);
- ax12.SetGoal(200, 1);
-
-}
\ No newline at end of file
+void Rx_interrupt();
+void SetSerial(int c);
+
+int state_of_ST1 = 0;
+int state_of_ST2 = 0;
+int state_of_ST3 = 0;
+
+float u_j1=0;
+float u_j2=0;
+float u_j3=0;
+
+void drive_motor_1();
+void drive_motor_2();
+void drive_motor_3();
+void calculation();
+
+//GREEN-------------------------------------------------------------
+//motor set 1
+DigitalOut ENA_1(PB_14);
+DigitalOut DIR_1(PC_4);
+DigitalOut PUL_1(PB_13);
+
+//motor set 2
+DigitalOut ENA_2(PB_2);
+DigitalOut DIR_2(PB_15);
+DigitalOut PUL_2(PB_1);
+
+//motor set 3
+DigitalOut ENA_3(PC_8);
+DigitalOut DIR_3(PC_5);
+DigitalOut PUL_3(PC_6);
+
+//Buzzer
+DigitalOut Buzzer(PA_12);
+
+//Vacum
+DigitalOut VACUM(PB_12);
+
+//Servo
+
+//Limit Switch
+DigitalIn LSwitch_1(PC_0);
+DigitalIn LSwitch_2(PC_1);
+DigitalIn LSwitch_3(PB_0);
+DigitalIn Home_Switch(PA_1);
+
+// SET VELOCITY
+float VelocityST_1 = 30; // rpm
+float VelocityST_2 = 100; // rpm
+float VelocityST_3 = 100; // rpm
+
+void open_vacum(){
+ VACUM = 1;
+}
+void close_vacum(){
+ VACUM = 0;
+}
+void open_buzzer(){
+ Buzzer = 1;
+}
+void close_buzzer(){
+ Buzzer = 0;
+}
+void set_home(){
+ if (LSwitch_2.read() != 0){
+ state_of_ST2=2;
+ u_j2=180;
+ drive_motor_2();
+ }
+ if (LSwitch_1.read() != 0){
+ state_of_ST1 =2;
+ u_j1=180;
+ drive_motor_1();
+ state_of_ST1 =1;
+ u_j1=45;
+ drive_motor_1();
+ }
+ if (LSwitch_3.read() != 0){
+ state_of_ST3=2;
+ u_j3=180;
+ drive_motor_3();
+ state_of_ST3=1;
+ u_j3=70;
+ drive_motor_3();
+ }
+ ax12.SetGoal(90, 1);
+}
+//GREEN-------------------------------------------------------------
+
+int data_size = 16;
+char data[16] = {};
+char package = 0;
+char num_data = 0;
+
+float q[4] = {};
+void ConvertData2q()
+{
+ char q_mode[4] = {data[3], data[6], data[9], data[12]};
+ char q_int[4] = {data[4], data[7], data[10], data[13]};
+ char q_dec[4] = {data[5], data[8], data[11], data[14]};
+ for(int i=0;i<4;i++)
+ {
+ if(q_mode[i] == 1)
+ {
+ q[i] = q_int[i]+q_dec[i]/100;
+ }
+ else if(q_mode[i] == 2)
+ {
+ q[i] = (-1)*(q_int[i]+q_dec[i]/100);
+ }
+ }
+}
+
+int main()
+{
+ device.baud(115200);
+ device.attach(&Rx_interrupt);
+ close_buzzer();
+ close_vacum();
+ //set_home();
+ while(1)
+ {
+ if (package == 1)
+ {
+ package = 0;
+ if(data[2]==1){
+ open_buzzer();
+ open_vacum();
+ set_home();
+ data[2]=0;
+ }
+ if(data[2]==2){
+ ConvertData2q();
+ state_of_ST1=data[3];
+ u_j1=abs(q[0]);
+ state_of_ST2=data[6];
+ u_j2=abs(q[1]);
+ state_of_ST3=data[9];
+ u_j3=abs(q[2]);
+ drive_motor_1();
+ drive_motor_2();
+ drive_motor_3();
+ TxEn = 1;
+ ax12.SetCRSpeed(0.1);
+ ax12.SetGoal(abs(q[3]), 1);
+ if(abs(q[3])==180){
+ open_vacum();
+ }
+ if(abs(q[3])==90){
+ wait(3);
+ close_vacum();
+ }
+ }
+ }
+ }
+}
+
+void Rx_interrupt()
+{
+ char c = device.getc();
+ int i = (int)c;
+ SetSerial(i);
+}
+void SetSerial(int c)
+{
+ if (num_data < 2){
+ if (c == 255){
+ data[num_data] = c;
+ num_data++;
+ }else{
+ data[num_data] = c;
+ num_data = 0;
+ }
+ }
+ else if (num_data < data_size){
+ data[num_data] = c;
+ num_data++;
+ if (num_data >= data_size){
+ if (data[data_size-1]==255){
+ num_data = 0;
+ package = 1;
+ }
+ else num_data = 0;
+ }
+ }
+}
+void drive_motor_1(){
+ float round_1 = u_j1 * 8000/360;//1:10 rpm x step pluse u_j1 default 60000
+ float pluseforST_1 =(60/(VelocityST_1*800))/2;
+ if (state_of_ST1==1){
+ ENA_1 = 1;
+ DIR_1 = 0;
+ for (int i=0; i< round_1; i++){
+ PUL_1 = 1;
+ wait(pluseforST_1);//default 0.0005
+ PUL_1 = 0;
+ wait(pluseforST_1);//default 0.0005
+ }
+ //state_of_ST1 = 0;
+ }
+ if (state_of_ST1==2){
+ ENA_1 = 1;
+ DIR_1 = 1;
+ for (int i=0; i< round_1; i++){
+ PUL_1 = 1;
+ wait(pluseforST_1);//default 0.0005
+ PUL_1 = 0;
+ wait(pluseforST_1);//default 0.0005
+ if (LSwitch_1.read() == 0)
+ {
+ break;
+ }
+ }
+ }
+ }
+void drive_motor_2(){
+ float round_2 = u_j2 * 40000/360;// 1:20rpm x step pluse u_j1 default 60000
+ float pluseforST_2 =(60/(VelocityST_2*800))/2;
+ if (state_of_ST2==1){
+ ENA_2 = 1;
+ DIR_2 = 1;
+ for (int i=0; i< round_2; i++){
+ PUL_2 = 1;
+ wait(pluseforST_2);//default 0.0005
+ PUL_2 = 0;
+ wait(pluseforST_2);//default 0.0005
+ }
+ //state_of_ST1 = 0;
+
+ }
+ if (state_of_ST2==2){
+ ENA_2 = 1;
+ DIR_2 = 0;
+ for (int i=0; i< round_2; i++){
+ PUL_2 = 1;
+ wait(pluseforST_2);//default 0.0005
+ PUL_2 = 0;
+ wait(pluseforST_2);//default 0.0005
+ if (LSwitch_2.read() == 0)
+ {
+ break;
+ }
+ }
+ }
+ }
+
+void drive_motor_3(){
+ float round_3 = u_j3 * 16000 /360;//1:40 rpm x step pluse u_j1 default 60000
+ float pluseforST_3 =(60/(VelocityST_3*800))/2;
+ if (state_of_ST3==1){
+ ENA_3 = 1;
+ DIR_3 = 0;
+ for (int i=0; i< round_3; i++){
+ PUL_3 = 1;
+ wait(pluseforST_3);//default 0.0005
+ PUL_3 = 0;
+ wait(pluseforST_3);//default 0.0005
+ }
+ //state_of_ST1 = 0;
+ }
+ if (state_of_ST3==2){
+ ENA_3 = 1;
+ DIR_3 = 1;
+ for (int i=0; i< round_3; i++){
+ PUL_3 = 1;
+ wait(pluseforST_3);//default 0.0005
+ PUL_3 = 0;
+ wait(pluseforST_3);//default 0.0005
+ if (LSwitch_3.read() == 0)
+ {
+ break;
+ }
+ }
+ }
+ }
\ No newline at end of file