Diff: main.cpp

Revision:

0:7f0b090897bc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 26 06:15:07 2016 +0000
@@ -0,0 +1,216 @@
+#include "mbed.h"
+
+#define PWMMAX 0.33
+#define stMAX 1023
+#define stMIN 0
+#define daMAX 255
+#define SERVO_MIN_ANG 1020 //us 0.0
+
+Serial comUSB(USBTX,USBRX);
+Serial com1(p9,p10);
+//Serial com1(p13, p14);
+
+DigitalIn angMAX(p18);
+DigitalIn angMIN(p19);
+DigitalOut bufcheck(LED1);
+DigitalOut dirR(p28);
+DigitalOut dirL(p29);
+DigitalOut dirA(p20);
+DigitalOut shoter(p11);
+DigitalOut loader(p12);
+PwmOut servo(p24);
+PwmOut pwrR(p26);
+PwmOut pwrL(p25);
+PwmOut pwrA(p21);
+
+const float angpwr = 0.18;
+int buf_count = -1;
+
+union Get_data{
+    char alldata[6];
+    struct{
+        char header :8;
+        unsigned int x :8;
+        unsigned int y :8;
+        unsigned int t :8;
+        int au :1;
+        int ad :1;
+        int h :1;
+        int set :1;
+        int s :1;
+        int vx :1;
+        int vy :1;
+        int vt :1;
+    }data;
+}buf_data;
+
+union union_data{
+    char alldata[9];
+    struct{
+        char header :8;
+        signed int x :16;
+        signed int y :16;
+        signed int t :16;
+        int au :1;
+        int ad :1;
+        int h :1;
+        int set :1;
+        int s :1;
+    }data;
+}act_data;
+
+
+
+void Serialsetup(){
+    comUSB.baud(9600);
+    com1.baud(115200);
+}
+
+void interface(){
+    char data;
+    bufcheck = 1;
+    while(com1.readable()){
+        data = com1.getc();
+        bufcheck = 1;
+        //comUSB.printf("%d\r\n",data);
+        if(data == 0xFE){
+            buf_count = 0;
+            
+        }
+       if(buf_count < 6 && buf_count != -1){
+            buf_data.alldata[buf_count] = data;
+            buf_count++;
+       }
+       
+    }
+    
+    if(buf_count == 6 && buf_data.alldata[0] == 0xFE){
+        if(buf_data.data.vx == 0){
+            act_data.data.x = (signed int)buf_data.data.x * -1;
+        }else{
+            act_data.data.x = buf_data.data.x;
+        }
+        
+        if(buf_data.data.vy == 0){
+            act_data.data.y = (signed int)buf_data.data.y * -1;
+        }else{
+            act_data.data.y = buf_data.data.y;
+        }
+        
+        if(buf_data.data.vt == 0){
+            act_data.data.t = (signed int)buf_data.data.t * -1;
+        }else{
+            act_data.data.t = buf_data.data.t;
+        }
+        act_data.data.au = buf_data.data.au;
+        act_data.data.ad = buf_data.data.ad;
+        act_data.data.h = buf_data.data.h;
+        act_data.data.set = buf_data.data.set;
+        act_data.data.s = buf_data.data.s;
+    }
+    
+            
+            
+}
+
+
+void drive_mech(){
+    float power,lduty,turn;
+    power = act_data.data.y * (PWMMAX / daMAX);
+    lduty = act_data.data.x * (PWMMAX / daMAX);
+    turn = act_data.data.t * (PWMMAX / daMAX);
+    if(power == 0.00){
+        if(turn < 0){
+            dirR = 1;
+            dirL = 1;
+            pwrR = turn * -1;
+            pwrL = turn * -1;
+        }else if(turn >= 0){
+            dirR = 0;
+            dirL = 0;
+            pwrR = turn;
+            pwrL = turn;    
+        }
+    }else{ 
+        if(power <= 0){
+            power = power * -1;
+            dirR = 1;
+            dirL = 0;
+        }else if (power > 0){
+            dirR = 0;
+            dirL = 1;
+        }
+    
+        if(lduty <= 0){
+            pwrR = power;
+            pwrL = power - (lduty * -1) / 2;   
+        }else if(lduty > 0){
+            pwrR = power - lduty / 2; 
+            pwrL = power;
+        }    
+        //comUSB.printf("p %f, l %f,t %f\r\n",power,lduty,turn);
+    }
+}
+
+
+void seiyaho(){
+    if(act_data.data.au == 1){
+        if(angMAX != 1){
+            dirA = 1;
+            pwrA = angpwr;
+        }else if(angMAX == 1){
+            pwrA = 0.00;
+        }
+    }else if(act_data.data.ad == 1){
+        if(angMIN != 1){
+            dirA = 0;
+            pwrA = angpwr;
+        }else if(angMIN == 1){
+            pwrA = 0.00;
+        }
+    }else if(act_data.data.au != 1 && act_data.data.ad != 1) {
+        pwrA = 0.00;
+    }
+            
+}
+
+
+void hold(){
+    if(act_data.data.h == 1 && act_data.data.s != 1 && act_data.data.s != 1){
+        servo.pulsewidth_us(SERVO_MIN_ANG + 900);
+    }else if(act_data.data.h != 1 && act_data.data.set != 1){
+        servo.pulsewidth_us(SERVO_MIN_ANG + 500 * (0 / 90));
+    }else if(act_data.data.set == 1 && act_data.data.s != 1 && act_data.data.s != 1){
+        servo.pulsewidth_us(SERVO_MIN_ANG + 1280);
+    }
+}
+
+
+void fire(){
+    if(act_data.data.s == 1 && act_data.data.h != 1 && act_data.data.set != 1){
+        loader = 0;
+        shoter = 1;
+    }else if(act_data.data.s == 0 && act_data.data.h != 1 && act_data.data.set != 1){
+        loader = 1;
+        shoter = 0;
+    }
+}
+
+
+int main() {
+        Serialsetup();
+        servo.period_ms(20);
+    while(1) {
+        bufcheck = 0;
+        com1.attach(interface,Serial::RxIrq);
+        fire();
+        drive_mech();
+        seiyaho();
+        hold();
+        /*comUSB.printf("x %d,y %d,t %d,u %d,d %d,h %d,%d,s ,%d %d,%d,%d\r\n",
+                        act_data.data.x,act_data.data.y,act_data.data.t,
+                        act_data.data.au,act_data.data.ad,act_data.data.h,act_data.data.set,
+                        act_data.data.s,buf_data.data.vx,buf_data.data.vy,buf_data.data.vt);*/
+        
+    }
+}
\ No newline at end of file