Chawisorn samrit / Mbed 2 deprecated M89_FULL_CODE

green

Dependencies:   mbed AX12

main.cpp@1:f72ac77b34f3, 2019-04-23 (annotated)

Committer:
csggreen
Date:
Tue Apr 23 05:35:31 2019 +0000
Revision:
1:f72ac77b34f3
Parent:
0:f4444dfcd74c 
green

Who changed what in which revision?

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