Diff: main.cpp

Revision:

1:496ab5127b06

Parent:

0:e06e8c048454

Child:

2:e2ec64c77b6e

--- a/main.cpp	Mon Oct 19 12:43:59 2020 +0000
+++ b/main.cpp	Tue Oct 27 09:33:35 2020 +0000
@@ -1,37 +1,38 @@
 #include "mbed.h"
 #include "pin_config.h"
 #include "QEI.h"
-
+#include "FEP.h"
 Serial pc(USBTX,USBRX,115200);
-//Serial serial(PC_10,PC_11,9600);
+RawSerial serial(PC_10,PC_11,9600);
 
 AnalogIn finger[]={
-    AnalogIn(A_IN_7),
-    AnalogIn(A_IN_6),
-    AnalogIn(A_IN_5),
+    AnalogIn(A_IN_1),
+    AnalogIn(A_IN_2),
     AnalogIn(A_IN_3),
-    AnalogIn(A_IN_2)
+    AnalogIn(A_IN_4),
+    AnalogIn(A_IN_5)
 };
 
-/*
 AnalogIn wrist_bent[]={
-    AnalogIn(A_IN_),
-    AnalogIn(A_IN_)
+    AnalogIn(A_IN_6),
+    AnalogIn(A_IN_7)
 };
 
 AnalogIn wrist_twist[]={
-    AnalogIn(A_IN_)
+    AnalogIn(A_IN_8)
 };
 
-AnalogIn elbow_bent[]={
-    AnalogIn(A_IN_)
+QEI elbow_bent[]={
+    QEI(QEI_1_A,QEI_1_B, NC, 360,QEI::X4_ENCODING)
 };
 
 QEI shoulder_rotation[]={
-    QEI(QEI_1_A,QEI_1_B, NC, 360,QEI::X4_ENCODING),
-    QEI(QEI_2_A,QEI_2_B, NC, 360,QEI::X4_ENCODING)
-}
-*/
+    QEI(QEI_2_A,QEI_2_B, NC, 360,QEI::X4_ENCODING),
+    QEI(QEI_3_A,QEI_3_B, NC, 360,QEI::X4_ENCODING)
+};
+
+FEP fep(PC_12,PD_2,003);
+
 DigitalIn b(USER_BUTTON);
 
 typedef struct {   
@@ -48,37 +49,33 @@
 };
 
 void status_provider::percentage_calculation(){
-    status.percentage = (1 - (status.now_val - status.close_val) / (status.open_val - status.close_val)) * 255;
+    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;
     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 = 255;
 }
 
 
 status_provider status_[11];
 
-void get_all_data(int mode){
+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<5;i++)status_[i].status.close_val = finger[i].read();/*
-        for(i=5;i<7;i++)status_[i].status.close_val = wrist_bent[i-5].read();
-        for(i=7;i<8;i++)status_[i].status.close_val = wrist_twist[i-7].read();
-        for(i=8;i<9;i++)status_[i].status.close_val = elbow_bent[i-8].read();
-        for(i=9;i<11;i++)status_[i].status.close_val = shoulder_rotation[i-9].getPulses();*/
+        for(i=0;i<11;i++)status_[i].status.close_val = val[i];
     }
     if(mode==2){
-        for(i=0;i<5;i++)status_[i].status.open_val = finger[i].read();/*
-        for(i=5;i<7;i++)status_[i].status.open_val = wrist_bent[i-5].read();
-        for(i=7;i<8;i++)status_[i].status.open_val = wrist_twist[i-7].read();
-        for(i=8;i<9;i++)status_[i].status.open_val = elbow_bent[i-8].read();
-        for(i=9;i<11;i++)status_[i].status.open_val = shoulder_rotation[i-9].getPulses();*/
+        for(i=0;i<11;i++)status_[i].status.open_val = val[i];
     }
     if(mode==3){
-        for(i=0;i<5;i++)status_[i].status.now_val = finger[i].read();/*
-        for(i=5;i<7;i++)status_[i].status.now_val = wrist_bent[i-5].read();
-        for(i=7;i<8;i++)status_[i].status.now_val = wrist_twist[i-7].read();
-        for(i=8;i<9;i++)status_[i].status.now_val = elbow_bent[i-8].read();
-        for(i=9;i<11;i++)status_[i].status.now_val = shoulder_rotation[i-9].getPulses();*/
+        for(i=0;i<11;i++)status_[i].status.now_val = val[i];
     }
 }
 int main()
@@ -88,18 +85,30 @@
     
     
     for(i=0;i<11;i++)data[i] = &status_[i].status.percentage;
-    get_all_data(1);
+    update_status(1);
     while(bool first_loop=1){
         bool b_ = b;
-        get_all_data(2);
+        update_status(2);
         if(!b_){
             while(bool second_loop=1){
-                get_all_data(3);
+                update_status(3);
                 for(i=0;i<11;i++){
                     status_[i].percentage_calculation();
-                    pc.printf("[%d]:%d,",i,*data[i]);
+                    /*
+                    if(b.read()==0){
+                        *data[8]=0;
+                    }
+                    else{
+                        *data[8]=255;
+                    }
+                    */
+                    
+                    pc.printf("[%2d]:%3d,",i,*data[i]);
+                    serial.printf("%d",*data[i]);
                 }
                 pc.printf("\n\r");
+                serial.printf("\r\n");
+                fep.sendData(data,11);
                 
             }