Use IQS62X sensor and move motor by detected angle
Dependencies: DRV8830 IQS62x IQSDisplayTerminal UIT_ACM1602NI mbed
Fork of Nucleo_ACM1602_I2C_DC by
main.cpp@7:acb1074eaba6, 2017-09-22 (annotated)
- Committer:
- 8mona
- Date:
- Fri Sep 22 16:27:18 2017 +0000
- Revision:
- 7:acb1074eaba6
- Parent:
- 6:e3afb1390167
- Child:
- 8:f7ad1d7176ba
Read Angle and speed register from IQS624 sensor
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MikamiUitOpen | 0:132eb47d57fb | 1 | //------------------------------------------------------------ |
MikamiUitOpen | 0:132eb47d57fb | 2 | // Demo program for LCD ACM1602NI using I2C interface |
MikamiUitOpen | 0:132eb47d57fb | 3 | // Pullup resistors for SDA and SCL: 4.7 kΩ |
MikamiUitOpen | 1:d5e9bd9b38ad | 4 | // 2016/04/01, Copyright (c) 2016 MIKAMI, Naoki |
MikamiUitOpen | 0:132eb47d57fb | 5 | //------------------------------------------------------------ |
MikamiUitOpen | 0:132eb47d57fb | 6 | |
MikamiUitOpen | 0:132eb47d57fb | 7 | #include "ACM1602NI.hpp" |
8mona | 3:6474ab60854e | 8 | #include "DRV8830.h" |
8mona | 5:2b9614aa1171 | 9 | #include "IQS62x.h" |
8mona | 5:2b9614aa1171 | 10 | #include "IQSdisplayTerminal.h" |
8mona | 5:2b9614aa1171 | 11 | |
8mona | 5:2b9614aa1171 | 12 | |
8mona | 5:2b9614aa1171 | 13 | //IQS62xDisplay terminal; // class to display IQS62x registers on a terminal |
8mona | 5:2b9614aa1171 | 14 | IQS62xIO iqs62x; // class for basic IQS62x block read and write |
8mona | 3:6474ab60854e | 15 | |
8mona | 2:ea066749e515 | 16 | //Cycle |
8mona | 5:2b9614aa1171 | 17 | #define UP_DURATION 14 //On time [*100ms] |
8mona | 5:2b9614aa1171 | 18 | #define WAIT_DELAY 10 //Delay time [*100ms] |
8mona | 5:2b9614aa1171 | 19 | #define DOWN_DURATION 8 //Down time [*50ms] |
8mona | 5:2b9614aa1171 | 20 | #define SWITCH_PERIOD 50 //Cycle time[*50ms] |
8mona | 5:2b9614aa1171 | 21 | #define TOTAL_TIMES 30000 //total times n |
8mona | 4:83f7df775d46 | 22 | #define ACCEL_SIZE 6 |
8mona | 5:2b9614aa1171 | 23 | #define DECEL_SIZE 6 |
8mona | 3:6474ab60854e | 24 | |
8mona | 3:6474ab60854e | 25 | |
8mona | 5:2b9614aa1171 | 26 | |
8mona | 5:2b9614aa1171 | 27 | const float spd_table[] = {1.0,0.9,0.8}; |
8mona | 5:2b9614aa1171 | 28 | //const float vol_accel[ACCEL_SIZE] = {0,0.3,0.6,0.8,0.95,1}; |
8mona | 5:2b9614aa1171 | 29 | const float vol_decel[DECEL_SIZE] = {1,0.75,0.4,0.15,0.05,0}; |
8mona | 5:2b9614aa1171 | 30 | const float vol_accel[ACCEL_SIZE] = {0,0.5,0.8,1,1,1}; |
8mona | 5:2b9614aa1171 | 31 | //const float vol_decel[DECEL_SIZE] = {1,0.75,0.15,-1,-0.2,0}; |
8mona | 5:2b9614aa1171 | 32 | |
8mona | 5:2b9614aa1171 | 33 | const float donw_ratio=0.85; |
8mona | 5:2b9614aa1171 | 34 | |
8mona | 3:6474ab60854e | 35 | static int sp_index=0; |
8mona | 2:ea066749e515 | 36 | |
8mona | 2:ea066749e515 | 37 | |
MikamiUitOpen | 0:132eb47d57fb | 38 | using namespace Mikami; |
8mona | 5:2b9614aa1171 | 39 | void ShowLCD(char * buffer, int startbyte, int endbyte); // for wheel output |
MikamiUitOpen | 0:132eb47d57fb | 40 | |
MikamiUitOpen | 0:132eb47d57fb | 41 | Acm1602Ni lcd_; // Default, OK |
8mona | 3:6474ab60854e | 42 | |
MikamiUitOpen | 0:132eb47d57fb | 43 | //Acm1602Ni lcd_(D14, D15); // OK |
MikamiUitOpen | 0:132eb47d57fb | 44 | //Acm1602Ni lcd_(D14, D15, 200000); // OK |
MikamiUitOpen | 0:132eb47d57fb | 45 | //Acm1602Ni lcd_(D14, D15, 200000, true, true); // OK |
MikamiUitOpen | 0:132eb47d57fb | 46 | //Acm1602Ni lcd_(PB_3, PB_10); // OK |
MikamiUitOpen | 0:132eb47d57fb | 47 | //Acm1602Ni lcd_(PC_9, PA_8); // OK |
MikamiUitOpen | 0:132eb47d57fb | 48 | //Acm1602Ni lcd_(PB_4, PA_8); // OK |
MikamiUitOpen | 0:132eb47d57fb | 49 | |
MikamiUitOpen | 0:132eb47d57fb | 50 | Ticker timer_; |
8mona | 3:6474ab60854e | 51 | I2C i2c(D14, D15); |
8mona | 3:6474ab60854e | 52 | DRV8830 motor(i2c, DRV8830ADDR_NN); |
8mona | 2:ea066749e515 | 53 | InterruptIn button1(USER_BUTTON); |
8mona | 3:6474ab60854e | 54 | static float motor_speed; |
MikamiUitOpen | 0:132eb47d57fb | 55 | |
8mona | 3:6474ab60854e | 56 | |
MikamiUitOpen | 0:132eb47d57fb | 57 | // Display elapsed time in minutes and seconds |
8mona | 5:2b9614aa1171 | 58 | |
8mona | 5:2b9614aa1171 | 59 | |
8mona | 7:acb1074eaba6 | 60 | int ReadDegree(char * buffer) |
8mona | 5:2b9614aa1171 | 61 | { |
8mona | 7:acb1074eaba6 | 62 | int ret=0; |
8mona | 7:acb1074eaba6 | 63 | |
8mona | 7:acb1074eaba6 | 64 | //(High bit + Low bit) * 360/65536 |
8mona | 7:acb1074eaba6 | 65 | //ret = ((buffer[0x80]<<8 + buffer[0x81])*0.00549316406 ; |
8mona | 7:acb1074eaba6 | 66 | ret = (buffer[0x80]<<8 +buffer[0x81])/65536.0*360.0 ; |
8mona | 7:acb1074eaba6 | 67 | return ret; |
8mona | 5:2b9614aa1171 | 68 | } |
8mona | 5:2b9614aa1171 | 69 | |
8mona | 5:2b9614aa1171 | 70 | |
8mona | 7:acb1074eaba6 | 71 | int ReadSpeed(char * buffer) |
8mona | 7:acb1074eaba6 | 72 | { |
8mona | 7:acb1074eaba6 | 73 | int ret=0; |
8mona | 7:acb1074eaba6 | 74 | ret = (buffer[0x8E]); |
8mona | 7:acb1074eaba6 | 75 | return ret; |
8mona | 7:acb1074eaba6 | 76 | } |
8mona | 7:acb1074eaba6 | 77 | |
8mona | 7:acb1074eaba6 | 78 | void Displaylevel (int deg) |
8mona | 7:acb1074eaba6 | 79 | { |
8mona | 7:acb1074eaba6 | 80 | |
8mona | 7:acb1074eaba6 | 81 | int level=deg>>5; |
8mona | 7:acb1074eaba6 | 82 | lcd_.WriteStringXY("@",0,0); |
8mona | 7:acb1074eaba6 | 83 | for (int i=0;i<12;i++) |
8mona | 7:acb1074eaba6 | 84 | { |
8mona | 7:acb1074eaba6 | 85 | if (i<level) |
8mona | 7:acb1074eaba6 | 86 | { |
8mona | 7:acb1074eaba6 | 87 | lcd_.WriteString("-"); |
8mona | 7:acb1074eaba6 | 88 | } |
8mona | 7:acb1074eaba6 | 89 | else |
8mona | 7:acb1074eaba6 | 90 | { |
8mona | 7:acb1074eaba6 | 91 | lcd_.WriteString(" "); |
8mona | 7:acb1074eaba6 | 92 | } |
8mona | 7:acb1074eaba6 | 93 | } |
8mona | 7:acb1074eaba6 | 94 | |
8mona | 7:acb1074eaba6 | 95 | |
8mona | 7:acb1074eaba6 | 96 | |
8mona | 7:acb1074eaba6 | 97 | } |
8mona | 7:acb1074eaba6 | 98 | |
8mona | 5:2b9614aa1171 | 99 | |
MikamiUitOpen | 0:132eb47d57fb | 100 | void TimerIsr() |
MikamiUitOpen | 0:132eb47d57fb | 101 | { |
8mona | 3:6474ab60854e | 102 | |
8mona | 3:6474ab60854e | 103 | //For LED Time-Sec display |
MikamiUitOpen | 0:132eb47d57fb | 104 | static int k = 0; |
8mona | 2:ea066749e515 | 105 | static char ctext[4]="---"; |
8mona | 2:ea066749e515 | 106 | div_t d_Cycle = div (k, SWITCH_PERIOD); |
8mona | 2:ea066749e515 | 107 | |
8mona | 2:ea066749e515 | 108 | //for Current time |
8mona | 5:2b9614aa1171 | 109 | div_t d_sec = div(k,600*2); //60s * 10n |
8mona | 2:ea066749e515 | 110 | int t_min = d_sec.quot; |
8mona | 2:ea066749e515 | 111 | div_t d_min = div(t_min,60); //1min=60s |
8mona | 2:ea066749e515 | 112 | int t_hr = d_min.quot; |
8mona | 2:ea066749e515 | 113 | |
8mona | 2:ea066749e515 | 114 | //for Current time |
8mona | 2:ea066749e515 | 115 | div_t df_sec = div(TOTAL_TIMES*SWITCH_PERIOD,600); //60s * 10n |
8mona | 2:ea066749e515 | 116 | int tf_min = df_sec.quot; |
8mona | 2:ea066749e515 | 117 | div_t df_min = div(tf_min,60); //1min=60s |
8mona | 2:ea066749e515 | 118 | int tf_hr = df_min.quot; |
8mona | 3:6474ab60854e | 119 | |
8mona | 3:6474ab60854e | 120 | |
8mona | 3:6474ab60854e | 121 | //Motor activation |
8mona | 2:ea066749e515 | 122 | |
8mona | 5:2b9614aa1171 | 123 | //Up Movement |
8mona | 5:2b9614aa1171 | 124 | if(WAIT_DELAY <= d_Cycle.rem && d_Cycle.rem < (WAIT_DELAY+UP_DURATION) ) |
8mona | 4:83f7df775d46 | 125 | { |
8mona | 4:83f7df775d46 | 126 | int accel_index = d_Cycle.rem - WAIT_DELAY; |
8mona | 4:83f7df775d46 | 127 | if (accel_index < ACCEL_SIZE) |
8mona | 4:83f7df775d46 | 128 | { |
8mona | 4:83f7df775d46 | 129 | motor_speed=vol_accel[accel_index] * spd_table[sp_index]; |
8mona | 4:83f7df775d46 | 130 | } |
8mona | 4:83f7df775d46 | 131 | else |
8mona | 4:83f7df775d46 | 132 | { |
8mona | 4:83f7df775d46 | 133 | motor_speed=spd_table[sp_index]; |
8mona | 4:83f7df775d46 | 134 | } |
8mona | 4:83f7df775d46 | 135 | strcpy(ctext," CW"); |
8mona | 4:83f7df775d46 | 136 | //ctext="CW"; |
8mona | 4:83f7df775d46 | 137 | |
8mona | 4:83f7df775d46 | 138 | } |
8mona | 4:83f7df775d46 | 139 | |
8mona | 5:2b9614aa1171 | 140 | //UP..stop |
8mona | 5:2b9614aa1171 | 141 | else if ( (WAIT_DELAY+UP_DURATION)<= d_Cycle.rem && d_Cycle.rem <(2*WAIT_DELAY+UP_DURATION) ) |
8mona | 3:6474ab60854e | 142 | { |
8mona | 4:83f7df775d46 | 143 | //wait_ms(20); |
8mona | 5:2b9614aa1171 | 144 | int accel_index = d_Cycle.rem - (WAIT_DELAY+UP_DURATION); |
8mona | 4:83f7df775d46 | 145 | if (accel_index< DECEL_SIZE) |
8mona | 4:83f7df775d46 | 146 | { |
8mona | 4:83f7df775d46 | 147 | motor_speed=vol_decel[accel_index] * spd_table[sp_index]; |
8mona | 4:83f7df775d46 | 148 | } |
8mona | 4:83f7df775d46 | 149 | else |
8mona | 4:83f7df775d46 | 150 | { |
8mona | 4:83f7df775d46 | 151 | motor_speed=0; |
8mona | 4:83f7df775d46 | 152 | } |
8mona | 4:83f7df775d46 | 153 | strcpy(ctext,"OFF"); |
8mona | 4:83f7df775d46 | 154 | } |
8mona | 4:83f7df775d46 | 155 | |
8mona | 5:2b9614aa1171 | 156 | |
8mona | 5:2b9614aa1171 | 157 | //down..Start |
8mona | 5:2b9614aa1171 | 158 | else if ( (2*WAIT_DELAY+UP_DURATION) <= d_Cycle.rem && d_Cycle.rem <(2*WAIT_DELAY+UP_DURATION+DOWN_DURATION) ) |
8mona | 4:83f7df775d46 | 159 | { |
8mona | 5:2b9614aa1171 | 160 | int accel_index = d_Cycle.rem - (2*WAIT_DELAY+UP_DURATION); |
8mona | 4:83f7df775d46 | 161 | if (accel_index < ACCEL_SIZE) |
8mona | 4:83f7df775d46 | 162 | { |
8mona | 5:2b9614aa1171 | 163 | motor_speed= - vol_accel[accel_index] * spd_table[sp_index]*donw_ratio; |
8mona | 4:83f7df775d46 | 164 | } |
8mona | 4:83f7df775d46 | 165 | else |
8mona | 4:83f7df775d46 | 166 | { |
8mona | 5:2b9614aa1171 | 167 | motor_speed= -spd_table[sp_index]*donw_ratio; |
8mona | 4:83f7df775d46 | 168 | } |
8mona | 3:6474ab60854e | 169 | strcpy(ctext," CW"); |
8mona | 3:6474ab60854e | 170 | //ctext="CW"; |
8mona | 3:6474ab60854e | 171 | } |
8mona | 3:6474ab60854e | 172 | |
8mona | 5:2b9614aa1171 | 173 | |
8mona | 5:2b9614aa1171 | 174 | //down..stop |
8mona | 5:2b9614aa1171 | 175 | else if ( (2*WAIT_DELAY+UP_DURATION+DOWN_DURATION) <= d_Cycle.rem ) |
8mona | 2:ea066749e515 | 176 | { |
8mona | 3:6474ab60854e | 177 | //wait_ms(20); |
8mona | 5:2b9614aa1171 | 178 | int accel_index = d_Cycle.rem - ((2*WAIT_DELAY+UP_DURATION+DOWN_DURATION)); |
8mona | 4:83f7df775d46 | 179 | if (accel_index< DECEL_SIZE) |
8mona | 4:83f7df775d46 | 180 | { |
8mona | 5:2b9614aa1171 | 181 | motor_speed= -vol_decel[accel_index] * spd_table[sp_index]*donw_ratio; |
8mona | 4:83f7df775d46 | 182 | } |
8mona | 4:83f7df775d46 | 183 | else |
8mona | 4:83f7df775d46 | 184 | { |
8mona | 4:83f7df775d46 | 185 | motor_speed=0; |
8mona | 4:83f7df775d46 | 186 | } |
8mona | 2:ea066749e515 | 187 | strcpy(ctext,"OFF"); |
8mona | 2:ea066749e515 | 188 | } |
8mona | 2:ea066749e515 | 189 | |
8mona | 2:ea066749e515 | 190 | |
8mona | 2:ea066749e515 | 191 | if(d_Cycle.quot==TOTAL_TIMES) |
8mona | 2:ea066749e515 | 192 | { |
8mona | 2:ea066749e515 | 193 | timer_.detach(); |
8mona | 2:ea066749e515 | 194 | } |
8mona | 3:6474ab60854e | 195 | |
8mona | 2:ea066749e515 | 196 | |
8mona | 2:ea066749e515 | 197 | |
MikamiUitOpen | 1:d5e9bd9b38ad | 198 | /* |
MikamiUitOpen | 0:132eb47d57fb | 199 | char str[20]; |
8mona | 2:ea066749e515 | 200 | sprintf(str, "%d'%2d\"", msec.quot, msec.rem); |
MikamiUitOpen | 0:132eb47d57fb | 201 | lcd_.WriteStringXY(str, 0, 1); |
MikamiUitOpen | 1:d5e9bd9b38ad | 202 | */ |
8mona | 2:ea066749e515 | 203 | |
8mona | 2:ea066749e515 | 204 | //1 Row |
8mona | 2:ea066749e515 | 205 | //lcd_.WriteStringXY("#",0,0); |
8mona | 3:6474ab60854e | 206 | |
8mona | 5:2b9614aa1171 | 207 | |
8mona | 5:2b9614aa1171 | 208 | |
8mona | 5:2b9614aa1171 | 209 | lcd_.WriteValueXY("%5d/", d_Cycle.quot,0,0); |
8mona | 5:2b9614aa1171 | 210 | lcd_.WriteValue("%0dM",TOTAL_TIMES); |
8mona | 5:2b9614aa1171 | 211 | lcd_.WriteValue("%1.2f", motor_speed); |
8mona | 5:2b9614aa1171 | 212 | //lcd_.WriteValue("V%0d",motor_speed); |
8mona | 3:6474ab60854e | 213 | |
8mona | 2:ea066749e515 | 214 | |
8mona | 2:ea066749e515 | 215 | //2 Row |
8mona | 2:ea066749e515 | 216 | lcd_.WriteValueXY("%03dh", t_hr, 0, 1); |
8mona | 2:ea066749e515 | 217 | lcd_.WriteValue("%02dm", d_min.rem); |
8mona | 2:ea066749e515 | 218 | lcd_.WriteValue("%03ds/", d_sec.rem); |
8mona | 2:ea066749e515 | 219 | lcd_.WriteValue("%03dh", tf_hr); |
8mona | 2:ea066749e515 | 220 | lcd_.WriteValue("%02dm", df_min.rem); |
8mona | 3:6474ab60854e | 221 | |
8mona | 2:ea066749e515 | 222 | k++; |
MikamiUitOpen | 0:132eb47d57fb | 223 | } |
MikamiUitOpen | 0:132eb47d57fb | 224 | |
8mona | 2:ea066749e515 | 225 | |
8mona | 2:ea066749e515 | 226 | |
8mona | 2:ea066749e515 | 227 | void flip() { |
8mona | 2:ea066749e515 | 228 | static bool b = false; |
8mona | 4:83f7df775d46 | 229 | |
8mona | 2:ea066749e515 | 230 | if(b==false) |
8mona | 2:ea066749e515 | 231 | { |
8mona | 4:83f7df775d46 | 232 | timer_.attach(&TimerIsr, 0.05); |
8mona | 2:ea066749e515 | 233 | } |
8mona | 2:ea066749e515 | 234 | |
8mona | 2:ea066749e515 | 235 | else |
8mona | 2:ea066749e515 | 236 | { |
8mona | 2:ea066749e515 | 237 | timer_.detach(); |
8mona | 3:6474ab60854e | 238 | //Relay1=0; |
8mona | 3:6474ab60854e | 239 | sp_index++; |
8mona | 3:6474ab60854e | 240 | if (sp_index == 3) |
8mona | 3:6474ab60854e | 241 | { |
8mona | 3:6474ab60854e | 242 | sp_index = 0; |
8mona | 3:6474ab60854e | 243 | } |
8mona | 2:ea066749e515 | 244 | } |
8mona | 2:ea066749e515 | 245 | b=!b; |
8mona | 2:ea066749e515 | 246 | |
8mona | 2:ea066749e515 | 247 | } |
8mona | 2:ea066749e515 | 248 | |
8mona | 2:ea066749e515 | 249 | |
MikamiUitOpen | 0:132eb47d57fb | 250 | int main() |
MikamiUitOpen | 0:132eb47d57fb | 251 | { |
8mona | 5:2b9614aa1171 | 252 | |
8mona | 3:6474ab60854e | 253 | //motor.speed(0); |
8mona | 5:2b9614aa1171 | 254 | // Check error and reset |
8mona | 3:6474ab60854e | 255 | |
8mona | 2:ea066749e515 | 256 | //LCD_cont=0; |
8mona | 3:6474ab60854e | 257 | //if (lcd_.IsConnected()) printf("\r\nConnected"); |
8mona | 3:6474ab60854e | 258 | //else printf("\r\nDisconnected"); |
MikamiUitOpen | 0:132eb47d57fb | 259 | |
8mona | 7:acb1074eaba6 | 260 | //TimerIsr(); |
8mona | 2:ea066749e515 | 261 | //timer_.attach(&TimerIsr, 0.1); |
8mona | 7:acb1074eaba6 | 262 | //button1.fall(&flip); |
8mona | 7:acb1074eaba6 | 263 | //lcd_.WriteStringXY("0",0,0); |
8mona | 7:acb1074eaba6 | 264 | lcd_.WriteStringXY("Boot1",0,0); |
8mona | 5:2b9614aa1171 | 265 | iqs62x.configure(); // configure the ICD |
8mona | 7:acb1074eaba6 | 266 | lcd_.WriteStringXY("Boot2",0,0); |
8mona | 5:2b9614aa1171 | 267 | |
8mona | 7:acb1074eaba6 | 268 | // bool status = motor.status(); |
8mona | 7:acb1074eaba6 | 269 | // if (status & DRV8830_F_FAULT){ |
8mona | 7:acb1074eaba6 | 270 | // motor.reset(); |
8mona | 7:acb1074eaba6 | 271 | // } |
8mona | 3:6474ab60854e | 272 | |
8mona | 3:6474ab60854e | 273 | while (true) { |
8mona | 7:acb1074eaba6 | 274 | iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers |
8mona | 7:acb1074eaba6 | 275 | int val = ReadDegree(iqs62x.registers); |
8mona | 7:acb1074eaba6 | 276 | int speed= ReadSpeed(iqs62x.registers); |
8mona | 7:acb1074eaba6 | 277 | |
8mona | 7:acb1074eaba6 | 278 | lcd_.WriteValueXY("T:%3d ",val, 0,1); |
8mona | 7:acb1074eaba6 | 279 | lcd_.WriteValue("V:%3d",speed); |
8mona | 7:acb1074eaba6 | 280 | Displaylevel(val); |
8mona | 7:acb1074eaba6 | 281 | wait_ms(5); |
8mona | 7:acb1074eaba6 | 282 | |
8mona | 5:2b9614aa1171 | 283 | } |
8mona | 5:2b9614aa1171 | 284 | |
MikamiUitOpen | 0:132eb47d57fb | 285 | } |
MikamiUitOpen | 0:132eb47d57fb | 286 |