Use IQS62X sensor and move motor by detected angle
Dependencies: DRV8830 IQS62x IQSDisplayTerminal UIT_ACM1602NI mbed
Fork of Nucleo_ACM1602_I2C_DC by
MotorMove.cpp@12:8464be95bf76, 2018-01-30 (annotated)
- Committer:
- 8mona
- Date:
- Tue Jan 30 06:55:27 2018 +0000
- Revision:
- 12:8464be95bf76
- Parent:
- 11:80b6c5d77073
Up version
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
8mona | 8:f7ad1d7176ba | 1 | #include "mbed.h" |
8mona | 8:f7ad1d7176ba | 2 | #include "DRV8830.h" |
8mona | 8:f7ad1d7176ba | 3 | #include "MotorMove.h" |
8mona | 8:f7ad1d7176ba | 4 | #define SPEED_RATIO 1.0 |
8mona | 8:f7ad1d7176ba | 5 | |
8mona | 8:f7ad1d7176ba | 6 | //float vol_decel_ini[DECEL_SIZE] = {1,0.75,0.4,0.15,0.05,0}; |
8mona | 8:f7ad1d7176ba | 7 | float vol_decel_ini[DECEL_SIZE] = {-0.5,0,0,0,0,0}; |
8mona | 12:8464be95bf76 | 8 | float vol_accel_ini[ACCEL_SIZE] = {1,1,1,1,1,1}; |
8mona | 12:8464be95bf76 | 9 | //float vol_accel_ini[ACCEL_SIZE] = {0.2,0.4,0.6,0.8,.9,1.0}; |
8mona | 8:f7ad1d7176ba | 10 | |
8mona | 9:b58e7d72a91c | 11 | void MotorMove::up_motor_set(int time_counter, float speed) |
8mona | 8:f7ad1d7176ba | 12 | { |
8mona | 9:b58e7d72a91c | 13 | bflag_up =1; |
8mona | 8:f7ad1d7176ba | 14 | bflag_down =0; |
8mona | 8:f7ad1d7176ba | 15 | start_time_count=time_counter; |
8mona | 9:b58e7d72a91c | 16 | accel_count = 0; |
8mona | 9:b58e7d72a91c | 17 | decel_count = 0; |
8mona | 8:f7ad1d7176ba | 18 | |
8mona | 8:f7ad1d7176ba | 19 | for (int i=0;i<ACCEL_SIZE;i++) |
8mona | 8:f7ad1d7176ba | 20 | { |
8mona | 9:b58e7d72a91c | 21 | vol_accel[i] = -vol_accel_ini[i]*speed/SPEED_RATIO; |
8mona | 8:f7ad1d7176ba | 22 | } |
8mona | 9:b58e7d72a91c | 23 | |
8mona | 8:f7ad1d7176ba | 24 | for (int i=0;i<DECEL_SIZE;i++) |
8mona | 8:f7ad1d7176ba | 25 | { |
8mona | 9:b58e7d72a91c | 26 | vol_decel[i] = -vol_decel_ini[i]*speed/SPEED_RATIO; |
8mona | 8:f7ad1d7176ba | 27 | } |
8mona | 8:f7ad1d7176ba | 28 | }; |
8mona | 8:f7ad1d7176ba | 29 | |
8mona | 9:b58e7d72a91c | 30 | void MotorMove::down_motor_set(int time_counter, float speed) |
8mona | 8:f7ad1d7176ba | 31 | { |
8mona | 9:b58e7d72a91c | 32 | bflag_up =0; |
8mona | 9:b58e7d72a91c | 33 | bflag_down =1; |
8mona | 8:f7ad1d7176ba | 34 | start_time_count=time_counter; |
8mona | 9:b58e7d72a91c | 35 | accel_count = 0; |
8mona | 9:b58e7d72a91c | 36 | decel_count = 0; |
8mona | 9:b58e7d72a91c | 37 | |
8mona | 8:f7ad1d7176ba | 38 | for (int i=0;i<ACCEL_SIZE;i++) |
8mona | 8:f7ad1d7176ba | 39 | { |
8mona | 8:f7ad1d7176ba | 40 | vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO; |
8mona | 8:f7ad1d7176ba | 41 | } |
8mona | 8:f7ad1d7176ba | 42 | |
8mona | 8:f7ad1d7176ba | 43 | for (int i=0;i<DECEL_SIZE;i++) |
8mona | 8:f7ad1d7176ba | 44 | { |
8mona | 8:f7ad1d7176ba | 45 | vol_decel[i] = vol_decel_ini[i]*speed/SPEED_RATIO; |
8mona | 8:f7ad1d7176ba | 46 | } |
8mona | 8:f7ad1d7176ba | 47 | }; |
8mona | 8:f7ad1d7176ba | 48 | |
8mona | 8:f7ad1d7176ba | 49 | |
8mona | 9:b58e7d72a91c | 50 | float MotorMove::ReturnMotorVol(int time_counter,int bflag_downed, int bflag_upped) |
8mona | 8:f7ad1d7176ba | 51 | { |
8mona | 8:f7ad1d7176ba | 52 | float speed; |
8mona | 9:b58e7d72a91c | 53 | int time; |
8mona | 8:f7ad1d7176ba | 54 | |
8mona | 9:b58e7d72a91c | 55 | static int flag_up_stopping=0; |
8mona | 9:b58e7d72a91c | 56 | static int flag_down_stopping=0; |
8mona | 8:f7ad1d7176ba | 57 | |
8mona | 9:b58e7d72a91c | 58 | //Check flag for normal mode or brake mode |
8mona | 9:b58e7d72a91c | 59 | //accel_count MODE |
8mona | 9:b58e7d72a91c | 60 | |
8mona | 8:f7ad1d7176ba | 61 | |
8mona | 9:b58e7d72a91c | 62 | if (bflag_up==1 && bflag_upped ==1) |
8mona | 9:b58e7d72a91c | 63 | { |
8mona | 9:b58e7d72a91c | 64 | flag_up_stopping=1; |
8mona | 9:b58e7d72a91c | 65 | //bflag_up==0; |
8mona | 9:b58e7d72a91c | 66 | } |
8mona | 9:b58e7d72a91c | 67 | else if (bflag_down==1 && bflag_downed ==1) |
8mona | 9:b58e7d72a91c | 68 | { |
8mona | 9:b58e7d72a91c | 69 | flag_down_stopping=1; |
8mona | 9:b58e7d72a91c | 70 | //bflag_down=0; |
8mona | 9:b58e7d72a91c | 71 | } |
8mona | 10:ef379cbc0004 | 72 | |
8mona | 10:ef379cbc0004 | 73 | |
8mona | 10:ef379cbc0004 | 74 | if (bflag_up==1 && bflag_upped==0) |
8mona | 10:ef379cbc0004 | 75 | { |
8mona | 10:ef379cbc0004 | 76 | if (accel_count<(ACCEL_SIZE-1)) |
8mona | 10:ef379cbc0004 | 77 | { |
8mona | 10:ef379cbc0004 | 78 | speed = vol_accel[accel_count]; |
8mona | 10:ef379cbc0004 | 79 | accel_count++; |
8mona | 10:ef379cbc0004 | 80 | } |
8mona | 10:ef379cbc0004 | 81 | else |
8mona | 10:ef379cbc0004 | 82 | { |
8mona | 10:ef379cbc0004 | 83 | speed = vol_accel[ACCEL_SIZE-1]; |
8mona | 10:ef379cbc0004 | 84 | } |
8mona | 10:ef379cbc0004 | 85 | } |
8mona | 10:ef379cbc0004 | 86 | else if(bflag_up==1 && bflag_upped==1) |
8mona | 10:ef379cbc0004 | 87 | { |
8mona | 10:ef379cbc0004 | 88 | speed=vol_decel[0]; |
8mona | 10:ef379cbc0004 | 89 | bflag_up=0; |
8mona | 10:ef379cbc0004 | 90 | } |
8mona | 10:ef379cbc0004 | 91 | else if (bflag_down==1 && bflag_downed==0) |
8mona | 10:ef379cbc0004 | 92 | { |
8mona | 10:ef379cbc0004 | 93 | if (accel_count<(ACCEL_SIZE-1)) |
8mona | 10:ef379cbc0004 | 94 | { |
8mona | 10:ef379cbc0004 | 95 | speed = vol_accel[accel_count]; |
8mona | 10:ef379cbc0004 | 96 | accel_count++; |
8mona | 10:ef379cbc0004 | 97 | } |
8mona | 10:ef379cbc0004 | 98 | else |
8mona | 10:ef379cbc0004 | 99 | { |
8mona | 10:ef379cbc0004 | 100 | speed = vol_accel[ACCEL_SIZE-1]; |
8mona | 10:ef379cbc0004 | 101 | } |
8mona | 10:ef379cbc0004 | 102 | } |
8mona | 10:ef379cbc0004 | 103 | else if(bflag_down==1 && bflag_downed==1) |
8mona | 10:ef379cbc0004 | 104 | { |
8mona | 10:ef379cbc0004 | 105 | speed=vol_decel[0]; |
8mona | 10:ef379cbc0004 | 106 | bflag_down=0; |
8mona | 10:ef379cbc0004 | 107 | } |
8mona | 10:ef379cbc0004 | 108 | |
8mona | 10:ef379cbc0004 | 109 | |
8mona | 10:ef379cbc0004 | 110 | |
8mona | 10:ef379cbc0004 | 111 | else |
8mona | 10:ef379cbc0004 | 112 | { |
8mona | 10:ef379cbc0004 | 113 | speed=0; |
8mona | 10:ef379cbc0004 | 114 | } |
8mona | 10:ef379cbc0004 | 115 | |
8mona | 10:ef379cbc0004 | 116 | /* |
8mona | 9:b58e7d72a91c | 117 | if (bflag_up==1) |
8mona | 9:b58e7d72a91c | 118 | { |
8mona | 9:b58e7d72a91c | 119 | if (flag_up_stopping==0) |
8mona | 9:b58e7d72a91c | 120 | { |
8mona | 9:b58e7d72a91c | 121 | if (accel_count<(ACCEL_SIZE-1)) |
8mona | 9:b58e7d72a91c | 122 | { |
8mona | 9:b58e7d72a91c | 123 | speed = vol_accel[accel_count]; |
8mona | 9:b58e7d72a91c | 124 | accel_count++; |
8mona | 9:b58e7d72a91c | 125 | } |
8mona | 9:b58e7d72a91c | 126 | else |
8mona | 9:b58e7d72a91c | 127 | { |
8mona | 9:b58e7d72a91c | 128 | speed = vol_accel[ACCEL_SIZE-1]; |
8mona | 9:b58e7d72a91c | 129 | } |
8mona | 9:b58e7d72a91c | 130 | } |
8mona | 9:b58e7d72a91c | 131 | else if(flag_up_stopping==1) |
8mona | 9:b58e7d72a91c | 132 | { |
8mona | 9:b58e7d72a91c | 133 | if (decel_count<=(DECEL_SIZE-1)) |
8mona | 9:b58e7d72a91c | 134 | { |
8mona | 9:b58e7d72a91c | 135 | speed = vol_decel[decel_count]; |
8mona | 9:b58e7d72a91c | 136 | decel_count++; |
8mona | 9:b58e7d72a91c | 137 | } |
8mona | 9:b58e7d72a91c | 138 | else |
8mona | 9:b58e7d72a91c | 139 | { |
8mona | 9:b58e7d72a91c | 140 | bflag_up=0; |
8mona | 9:b58e7d72a91c | 141 | speed=0; |
8mona | 9:b58e7d72a91c | 142 | flag_up_stopping=0; |
8mona | 9:b58e7d72a91c | 143 | } |
8mona | 8:f7ad1d7176ba | 144 | } |
8mona | 8:f7ad1d7176ba | 145 | } |
8mona | 9:b58e7d72a91c | 146 | else if (bflag_down==1) |
8mona | 9:b58e7d72a91c | 147 | { |
8mona | 9:b58e7d72a91c | 148 | if (flag_down_stopping==0) |
8mona | 9:b58e7d72a91c | 149 | { |
8mona | 9:b58e7d72a91c | 150 | if (accel_count<(ACCEL_SIZE-1)) |
8mona | 9:b58e7d72a91c | 151 | { |
8mona | 9:b58e7d72a91c | 152 | speed = vol_accel[accel_count]; |
8mona | 9:b58e7d72a91c | 153 | accel_count++; |
8mona | 9:b58e7d72a91c | 154 | } |
8mona | 9:b58e7d72a91c | 155 | else |
8mona | 9:b58e7d72a91c | 156 | { |
8mona | 9:b58e7d72a91c | 157 | speed = vol_accel[ACCEL_SIZE-1]; |
8mona | 9:b58e7d72a91c | 158 | } |
8mona | 9:b58e7d72a91c | 159 | } |
8mona | 9:b58e7d72a91c | 160 | else if(flag_down_stopping==1) |
8mona | 9:b58e7d72a91c | 161 | { |
8mona | 9:b58e7d72a91c | 162 | if (decel_count<=(DECEL_SIZE-1)) |
8mona | 9:b58e7d72a91c | 163 | { |
8mona | 9:b58e7d72a91c | 164 | speed = vol_decel[decel_count]; |
8mona | 9:b58e7d72a91c | 165 | decel_count++; |
8mona | 9:b58e7d72a91c | 166 | } |
8mona | 9:b58e7d72a91c | 167 | else |
8mona | 9:b58e7d72a91c | 168 | { |
8mona | 9:b58e7d72a91c | 169 | bflag_down=0; |
8mona | 9:b58e7d72a91c | 170 | speed=0; |
8mona | 9:b58e7d72a91c | 171 | flag_down_stopping=0; |
8mona | 9:b58e7d72a91c | 172 | } |
8mona | 9:b58e7d72a91c | 173 | } |
8mona | 9:b58e7d72a91c | 174 | } |
8mona | 9:b58e7d72a91c | 175 | else |
8mona | 8:f7ad1d7176ba | 176 | { |
8mona | 9:b58e7d72a91c | 177 | speed=0; |
8mona | 9:b58e7d72a91c | 178 | } |
8mona | 10:ef379cbc0004 | 179 | */ |
8mona | 9:b58e7d72a91c | 180 | |
8mona | 8:f7ad1d7176ba | 181 | |
8mona | 9:b58e7d72a91c | 182 | //move motor before push stopping switch |
8mona | 9:b58e7d72a91c | 183 | return speed; |
8mona | 9:b58e7d72a91c | 184 | |
8mona | 8:f7ad1d7176ba | 185 | |
8mona | 8:f7ad1d7176ba | 186 | }; |
8mona | 8:f7ad1d7176ba | 187 | |
8mona | 8:f7ad1d7176ba | 188 | |
8mona | 8:f7ad1d7176ba | 189 |