Kouki Suzuki
/
NHK2020_master
NHK2020_master_program
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?
User | Revision | Line number | New 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 | } |