Kouki Suzuki / Mbed 2 deprecated NHK2020_master

NHK2020_master_program

Dependencies:   mbed QEI

main.cpp@1:496ab5127b06, 2020-10-27 (annotated)

Committer:
skouki
Date:
Tue Oct 27 09:33:35 2020 +0000
Revision:
1:496ab5127b06
Parent:
0:e06e8c048454
Child:
2:e2ec64c77b6e
2020_10_27;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skouki 0:e06e8c048454 1 #include "mbed.h"
skouki 0:e06e8c048454 2 #include "pin_config.h"
skouki 0:e06e8c048454 3 #include "QEI.h"
skouki 1:496ab5127b06 4 #include "FEP.h"
skouki 0:e06e8c048454 5 Serial pc(USBTX,USBRX,115200);
skouki 1:496ab5127b06 6 RawSerial serial(PC_10,PC_11,9600);
skouki 0:e06e8c048454 7
skouki 0:e06e8c048454 8 AnalogIn finger[]={
skouki 1:496ab5127b06 9 AnalogIn(A_IN_1),
skouki 1:496ab5127b06 10 AnalogIn(A_IN_2),
skouki 0:e06e8c048454 11 AnalogIn(A_IN_3),
skouki 1:496ab5127b06 12 AnalogIn(A_IN_4),
skouki 1:496ab5127b06 13 AnalogIn(A_IN_5)
skouki 0:e06e8c048454 14 };
skouki 0:e06e8c048454 15
skouki 0:e06e8c048454 16 AnalogIn wrist_bent[]={
skouki 1:496ab5127b06 17 AnalogIn(A_IN_6),
skouki 1:496ab5127b06 18 AnalogIn(A_IN_7)
skouki 0:e06e8c048454 19 };
skouki 0:e06e8c048454 20
skouki 0:e06e8c048454 21 AnalogIn wrist_twist[]={
skouki 1:496ab5127b06 22 AnalogIn(A_IN_8)
skouki 0:e06e8c048454 23 };
skouki 0:e06e8c048454 24
skouki 1:496ab5127b06 25 QEI elbow_bent[]={
skouki 1:496ab5127b06 26 QEI(QEI_1_A,QEI_1_B, NC, 360,QEI::X4_ENCODING)
skouki 0:e06e8c048454 27 };
skouki 0:e06e8c048454 28
skouki 0:e06e8c048454 29 QEI shoulder_rotation[]={
skouki 1:496ab5127b06 30 QEI(QEI_2_A,QEI_2_B, NC, 360,QEI::X4_ENCODING),
skouki 1:496ab5127b06 31 QEI(QEI_3_A,QEI_3_B, NC, 360,QEI::X4_ENCODING)
skouki 1:496ab5127b06 32 };
skouki 1:496ab5127b06 33
skouki 1:496ab5127b06 34 FEP fep(PC_12,PD_2,003);
skouki 1:496ab5127b06 35
skouki 0:e06e8c048454 36 DigitalIn b(USER_BUTTON);
skouki 0:e06e8c048454 37
skouki 0:e06e8c048454 38 typedef struct {
skouki 0:e06e8c048454 39 float close_val;
skouki 0:e06e8c048454 40 float open_val;
skouki 0:e06e8c048454 41 float now_val;
skouki 0:e06e8c048454 42 uint8_t percentage;
skouki 0:e06e8c048454 43 } status_t;
skouki 0:e06e8c048454 44
skouki 0:e06e8c048454 45 class status_provider{
skouki 0:e06e8c048454 46 public:
skouki 0:e06e8c048454 47 status_t status;
skouki 0:e06e8c048454 48 void percentage_calculation();
skouki 0:e06e8c048454 49 };
skouki 0:e06e8c048454 50
skouki 0:e06e8c048454 51 void status_provider::percentage_calculation(){
skouki 1:496ab5127b06 52 status.percentage = (1 - ((status.now_val - status.close_val) / (status.open_val - status.close_val))) * 255;
skouki 1:496ab5127b06 53 //if(fabsf(status.open_val - status.close_val) >= 10.0f)return;
skouki 0:e06e8c048454 54 if(status.open_val <= status.now_val) status.percentage = 0;
skouki 1:496ab5127b06 55 if(status.close_val >=status.now_val) status.percentage = 255;
skouki 0:e06e8c048454 56 }
skouki 0:e06e8c048454 57
skouki 0:e06e8c048454 58
skouki 0:e06e8c048454 59 status_provider status_[11];
skouki 0:e06e8c048454 60
skouki 1:496ab5127b06 61 void update_status(int mode){
skouki 0:e06e8c048454 62 int i;
skouki 1:496ab5127b06 63 float val[11];
skouki 1:496ab5127b06 64
skouki 1:496ab5127b06 65 for(i=0;i<5;i++)val[i] = finger[i].read();
skouki 1:496ab5127b06 66 for(i=5;i<7;i++)val[i] = wrist_bent[i-5].read();
skouki 1:496ab5127b06 67 for(i=7;i<8;i++)val[i] = wrist_twist[i-7].read();
skouki 1:496ab5127b06 68 for(i=8;i<9;i++)val[i] = (float)(elbow_bent[i-8].getPulses());
skouki 1:496ab5127b06 69 for(i=9;i<11;i++)val[i] = (float)(shoulder_rotation[i-9].getPulses());
skouki 0:e06e8c048454 70
skouki 0:e06e8c048454 71 if(mode==1){
skouki 1:496ab5127b06 72 for(i=0;i<11;i++)status_[i].status.close_val = val[i];
skouki 0:e06e8c048454 73 }
skouki 0:e06e8c048454 74 if(mode==2){
skouki 1:496ab5127b06 75 for(i=0;i<11;i++)status_[i].status.open_val = val[i];
skouki 0:e06e8c048454 76 }
skouki 0:e06e8c048454 77 if(mode==3){
skouki 1:496ab5127b06 78 for(i=0;i<11;i++)status_[i].status.now_val = val[i];
skouki 0:e06e8c048454 79 }
skouki 0:e06e8c048454 80 }
skouki 0:e06e8c048454 81 int main()
skouki 0:e06e8c048454 82 {
skouki 0:e06e8c048454 83 uint8_t *data[11];
skouki 0:e06e8c048454 84 int i;
skouki 0:e06e8c048454 85
skouki 0:e06e8c048454 86
skouki 0:e06e8c048454 87 for(i=0;i<11;i++)data[i] = &status_[i].status.percentage;
skouki 1:496ab5127b06 88 update_status(1);
skouki 0:e06e8c048454 89 while(bool first_loop=1){
skouki 0:e06e8c048454 90 bool b_ = b;
skouki 1:496ab5127b06 91 update_status(2);
skouki 0:e06e8c048454 92 if(!b_){
skouki 0:e06e8c048454 93 while(bool second_loop=1){
skouki 1:496ab5127b06 94 update_status(3);
skouki 0:e06e8c048454 95 for(i=0;i<11;i++){
skouki 0:e06e8c048454 96 status_[i].percentage_calculation();
skouki 1:496ab5127b06 97 /*
skouki 1:496ab5127b06 98 if(b.read()==0){
skouki 1:496ab5127b06 99 *data[8]=0;
skouki 1:496ab5127b06 100 }
skouki 1:496ab5127b06 101 else{
skouki 1:496ab5127b06 102 *data[8]=255;
skouki 1:496ab5127b06 103 }
skouki 1:496ab5127b06 104 */
skouki 1:496ab5127b06 105
skouki 1:496ab5127b06 106 pc.printf("[%2d]:%3d,",i,*data[i]);
skouki 1:496ab5127b06 107 serial.printf("%d",*data[i]);
skouki 0:e06e8c048454 108 }
skouki 0:e06e8c048454 109 pc.printf("\n\r");
skouki 1:496ab5127b06 110 serial.printf("\r\n");
skouki 1:496ab5127b06 111 fep.sendData(data,11);
skouki 0:e06e8c048454 112
skouki 0:e06e8c048454 113 }
skouki 0:e06e8c048454 114
skouki 0:e06e8c048454 115 }
skouki 0:e06e8c048454 116 }
skouki 0:e06e8c048454 117 return 0;
skouki 0:e06e8c048454 118 }