Diff: main.cpp

Revision:

2:e2ec64c77b6e

Parent:

1:496ab5127b06

--- a/main.cpp	Tue Oct 27 09:33:35 2020 +0000
+++ b/main.cpp	Sat Nov 07 00:43:06 2020 +0000
@@ -1,25 +1,32 @@
 #include "mbed.h"
 #include "pin_config.h"
 #include "QEI.h"
-#include "FEP.h"
+
+#define MAX_VAL 9
+#define MAGNIFICATION 11
+
+#define MASTER_DATA_LENGTH 10
+#define CONTROLLER_DATA_LENGTH 3
+#define DATA_LENGTH 13
+
 Serial pc(USBTX,USBRX,115200);
-RawSerial serial(PC_10,PC_11,9600);
+Serial serial(PC_10,PC_11,115200);
 
 AnalogIn finger[]={
+    AnalogIn(A_IN_0),
     AnalogIn(A_IN_1),
     AnalogIn(A_IN_2),
     AnalogIn(A_IN_3),
-    AnalogIn(A_IN_4),
-    AnalogIn(A_IN_5)
+    AnalogIn(A_IN_4)
 };
 
 AnalogIn wrist_bent[]={
-    AnalogIn(A_IN_6),
-    AnalogIn(A_IN_7)
+    AnalogIn(A_IN_5),
+    AnalogIn(A_IN_6)
 };
 
 AnalogIn wrist_twist[]={
-    AnalogIn(A_IN_8)
+    AnalogIn(A_IN_7)
 };
 
 QEI elbow_bent[]={
@@ -27,11 +34,19 @@
 };
 
 QEI shoulder_rotation[]={
-    QEI(QEI_2_A,QEI_2_B, NC, 360,QEI::X4_ENCODING),
-    QEI(QEI_3_A,QEI_3_B, NC, 360,QEI::X4_ENCODING)
+    QEI(QEI_3_A,QEI_3_B, NC, 360,QEI::X4_ENCODING),
+    QEI(QEI_2_A,QEI_2_B, NC, 360,QEI::X4_ENCODING)
 };
 
-FEP fep(PC_12,PD_2,003);
+AnalogIn controller_stick[]={
+    AnalogIn(A_IN_8),
+    AnalogIn(A_IN_9)
+};
+
+DigitalIn controller_button[]={
+    DigitalIn(D_IN_0),
+    DigitalIn(D_IN_1)
+};
 
 DigitalIn b(USER_BUTTON);
 
@@ -46,72 +61,85 @@
     public:
     status_t status;
     void percentage_calculation();
+    
+    bool count;
 };
 
 void status_provider::percentage_calculation(){
-    status.percentage = (1 - ((status.now_val - status.close_val) / (status.open_val - status.close_val))) * 255;
-    //if(fabsf(status.open_val - status.close_val) >= 10.0f)return;
+
+    
+    status.percentage = (1 - ((status.now_val - status.close_val) / (status.open_val - status.close_val))) * MAX_VAL;
     if(status.open_val <= status.now_val) status.percentage = 0;
-    if(status.close_val >=status.now_val) status.percentage = 255;
+    if(status.close_val >=status.now_val) status.percentage = MAX_VAL;
+}
+
+
+status_provider status_[DATA_LENGTH];
+
+void update_status(int config_num = 0){
+    int i;
+    float val[MASTER_DATA_LENGTH];
+    
+    for(i=0;i<5;i++)val[i] = finger[i].read();
+    for(i=5;i<7;i++)val[i] = (float)(shoulder_rotation[i-5].getPulses());
+    for(i=7;i<8;i++)val[i] = (float)(elbow_bent[i-7].getPulses());
+    for(i=9;i<10;i++)val[i] = wrist_bent[0].read();
+    
+    for(i=0;i<MASTER_DATA_LENGTH;i++)status_[i].status.now_val = val[i];
+    if(config_num == 0)return;
+    for(i=0;i<MASTER_DATA_LENGTH;i++)status_[i].status.open_val = val[i];
+    if(config_num == 1)return;
+    for(i=0;i<MASTER_DATA_LENGTH;i++)status_[i].status.close_val = val[i];
+
+}
+
+void updata_controller_status(){
+    status_[MASTER_DATA_LENGTH].status.percentage = controller_stick[0].read() * MAX_VAL * MAGNIFICATION;
+    status_[MASTER_DATA_LENGTH+1].status.percentage = controller_stick[1].read() * MAX_VAL * MAGNIFICATION;
+    status_[MASTER_DATA_LENGTH+2].status.percentage = abs(1-controller_button[1]) + 1;
 }
 
 
-status_provider status_[11];
-
-void update_status(int mode){
-    int i;
-    float val[11];
-    
-    for(i=0;i<5;i++)val[i] = finger[i].read();
-    for(i=5;i<7;i++)val[i] = wrist_bent[i-5].read();
-    for(i=7;i<8;i++)val[i] = wrist_twist[i-7].read();
-    for(i=8;i<9;i++)val[i] = (float)(elbow_bent[i-8].getPulses());
-    for(i=9;i<11;i++)val[i] = (float)(shoulder_rotation[i-9].getPulses());
-    
-    if(mode==1){
-        for(i=0;i<11;i++)status_[i].status.close_val = val[i];
-    }
-    if(mode==2){
-        for(i=0;i<11;i++)status_[i].status.open_val = val[i];
-    }
-    if(mode==3){
-        for(i=0;i<11;i++)status_[i].status.now_val = val[i];
-    }
-}
 int main()
 {
-    uint8_t *data[11];
-    int i;
-    
+    controller_button[0].mode(PullUp);
+    controller_button[1].mode(PullUp);
+
+    uint8_t *data[DATA_LENGTH];
+    uint8_t i;
+    uint8_t sum;
+    for(i=0;i<DATA_LENGTH;i++)data[i] = &status_[i].status.percentage;
     
-    for(i=0;i<11;i++)data[i] = &status_[i].status.percentage;
-    update_status(1);
-    while(bool first_loop=1){
+    update_status(2);
+    while(bool config_loop = true){
         bool b_ = b;
-        update_status(2);
-        if(!b_){
-            while(bool second_loop=1){
-                update_status(3);
-                for(i=0;i<11;i++){
+        update_status(1);
+        if(!controller_button[0].read()||!b_){
+            while(bool main_loop = true){
+                
+                update_status();
+                sum = 0;
+                for(i=0;i<MASTER_DATA_LENGTH;i++){
                     status_[i].percentage_calculation();
-                    /*
-                    if(b.read()==0){
-                        *data[8]=0;
-                    }
-                    else{
-                        *data[8]=255;
-                    }
-                    */
-                    
-                    pc.printf("[%2d]:%3d,",i,*data[i]);
-                    serial.printf("%d",*data[i]);
                 }
+                
+                updata_controller_status();
+                
+                
+                *data[8]=0;
+                
+                for(i=0;i<DATA_LENGTH;i++){
+                    sum += *data[i];
+                    pc.printf("[%2d]:%2d,",i,*data[i]);
+                    serial.printf("%d,",*data[i]);   
+                }
+                
+                pc.printf("[sum]:%3d",sum);
+                //pc.printf("%5.2f",wrist_bent[0].read());
                 pc.printf("\n\r");
-                serial.printf("\r\n");
-                fep.sendData(data,11);
-                
+                serial.printf("%d,",sum);
+                serial.printf("A");
             }
-                
         }
     }
     return 0;