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@5:2b9614aa1171, 2017-09-21 (annotated)
- Committer:
- 8mona
- Date:
- Thu Sep 21 11:27:10 2017 +0000
- Revision:
- 5:2b9614aa1171
- Parent:
- 4:83f7df775d46
- Child:
- 6:e3afb1390167
Add S curve direction
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 | 3:6474ab60854e | 53 | //DigitalOut Relay1(D2); |
8mona | 2:ea066749e515 | 54 | InterruptIn button1(USER_BUTTON); |
8mona | 3:6474ab60854e | 55 | static float motor_speed; |
MikamiUitOpen | 0:132eb47d57fb | 56 | |
8mona | 3:6474ab60854e | 57 | |
MikamiUitOpen | 0:132eb47d57fb | 58 | // Display elapsed time in minutes and seconds |
8mona | 5:2b9614aa1171 | 59 | |
8mona | 5:2b9614aa1171 | 60 | |
8mona | 5:2b9614aa1171 | 61 | void ShowLCD(char * buffer, int startbyte, int endbyte) |
8mona | 5:2b9614aa1171 | 62 | { |
8mona | 5:2b9614aa1171 | 63 | for (int i=startbyte; i<=endbyte; i++) { |
8mona | 5:2b9614aa1171 | 64 | lcd_.WriteValue("%02x ", buffer[i]); // print out in black & white |
8mona | 5:2b9614aa1171 | 65 | } |
8mona | 5:2b9614aa1171 | 66 | } |
8mona | 5:2b9614aa1171 | 67 | |
8mona | 5:2b9614aa1171 | 68 | |
8mona | 5:2b9614aa1171 | 69 | |
MikamiUitOpen | 0:132eb47d57fb | 70 | void TimerIsr() |
MikamiUitOpen | 0:132eb47d57fb | 71 | { |
8mona | 3:6474ab60854e | 72 | |
8mona | 3:6474ab60854e | 73 | //For LED Time-Sec display |
MikamiUitOpen | 0:132eb47d57fb | 74 | static int k = 0; |
8mona | 2:ea066749e515 | 75 | static char ctext[4]="---"; |
8mona | 2:ea066749e515 | 76 | div_t d_Cycle = div (k, SWITCH_PERIOD); |
8mona | 2:ea066749e515 | 77 | |
8mona | 2:ea066749e515 | 78 | //for Current time |
8mona | 5:2b9614aa1171 | 79 | div_t d_sec = div(k,600*2); //60s * 10n |
8mona | 2:ea066749e515 | 80 | int t_min = d_sec.quot; |
8mona | 2:ea066749e515 | 81 | div_t d_min = div(t_min,60); //1min=60s |
8mona | 2:ea066749e515 | 82 | int t_hr = d_min.quot; |
8mona | 2:ea066749e515 | 83 | |
8mona | 2:ea066749e515 | 84 | //for Current time |
8mona | 2:ea066749e515 | 85 | div_t df_sec = div(TOTAL_TIMES*SWITCH_PERIOD,600); //60s * 10n |
8mona | 2:ea066749e515 | 86 | int tf_min = df_sec.quot; |
8mona | 2:ea066749e515 | 87 | div_t df_min = div(tf_min,60); //1min=60s |
8mona | 2:ea066749e515 | 88 | int tf_hr = df_min.quot; |
8mona | 3:6474ab60854e | 89 | |
8mona | 3:6474ab60854e | 90 | |
8mona | 3:6474ab60854e | 91 | //Motor activation |
8mona | 2:ea066749e515 | 92 | |
8mona | 5:2b9614aa1171 | 93 | //Up Movement |
8mona | 5:2b9614aa1171 | 94 | if(WAIT_DELAY <= d_Cycle.rem && d_Cycle.rem < (WAIT_DELAY+UP_DURATION) ) |
8mona | 4:83f7df775d46 | 95 | { |
8mona | 4:83f7df775d46 | 96 | int accel_index = d_Cycle.rem - WAIT_DELAY; |
8mona | 4:83f7df775d46 | 97 | if (accel_index < ACCEL_SIZE) |
8mona | 4:83f7df775d46 | 98 | { |
8mona | 4:83f7df775d46 | 99 | motor_speed=vol_accel[accel_index] * spd_table[sp_index]; |
8mona | 4:83f7df775d46 | 100 | } |
8mona | 4:83f7df775d46 | 101 | else |
8mona | 4:83f7df775d46 | 102 | { |
8mona | 4:83f7df775d46 | 103 | motor_speed=spd_table[sp_index]; |
8mona | 4:83f7df775d46 | 104 | } |
8mona | 4:83f7df775d46 | 105 | strcpy(ctext," CW"); |
8mona | 4:83f7df775d46 | 106 | //ctext="CW"; |
8mona | 4:83f7df775d46 | 107 | |
8mona | 4:83f7df775d46 | 108 | } |
8mona | 4:83f7df775d46 | 109 | |
8mona | 5:2b9614aa1171 | 110 | //UP..stop |
8mona | 5:2b9614aa1171 | 111 | else if ( (WAIT_DELAY+UP_DURATION)<= d_Cycle.rem && d_Cycle.rem <(2*WAIT_DELAY+UP_DURATION) ) |
8mona | 3:6474ab60854e | 112 | { |
8mona | 4:83f7df775d46 | 113 | //wait_ms(20); |
8mona | 5:2b9614aa1171 | 114 | int accel_index = d_Cycle.rem - (WAIT_DELAY+UP_DURATION); |
8mona | 4:83f7df775d46 | 115 | if (accel_index< DECEL_SIZE) |
8mona | 4:83f7df775d46 | 116 | { |
8mona | 4:83f7df775d46 | 117 | motor_speed=vol_decel[accel_index] * spd_table[sp_index]; |
8mona | 4:83f7df775d46 | 118 | } |
8mona | 4:83f7df775d46 | 119 | else |
8mona | 4:83f7df775d46 | 120 | { |
8mona | 4:83f7df775d46 | 121 | motor_speed=0; |
8mona | 4:83f7df775d46 | 122 | } |
8mona | 4:83f7df775d46 | 123 | strcpy(ctext,"OFF"); |
8mona | 4:83f7df775d46 | 124 | } |
8mona | 4:83f7df775d46 | 125 | |
8mona | 5:2b9614aa1171 | 126 | |
8mona | 5:2b9614aa1171 | 127 | //down..Start |
8mona | 5:2b9614aa1171 | 128 | else if ( (2*WAIT_DELAY+UP_DURATION) <= d_Cycle.rem && d_Cycle.rem <(2*WAIT_DELAY+UP_DURATION+DOWN_DURATION) ) |
8mona | 4:83f7df775d46 | 129 | { |
8mona | 5:2b9614aa1171 | 130 | int accel_index = d_Cycle.rem - (2*WAIT_DELAY+UP_DURATION); |
8mona | 4:83f7df775d46 | 131 | if (accel_index < ACCEL_SIZE) |
8mona | 4:83f7df775d46 | 132 | { |
8mona | 5:2b9614aa1171 | 133 | motor_speed= - vol_accel[accel_index] * spd_table[sp_index]*donw_ratio; |
8mona | 4:83f7df775d46 | 134 | } |
8mona | 4:83f7df775d46 | 135 | else |
8mona | 4:83f7df775d46 | 136 | { |
8mona | 5:2b9614aa1171 | 137 | motor_speed= -spd_table[sp_index]*donw_ratio; |
8mona | 4:83f7df775d46 | 138 | } |
8mona | 3:6474ab60854e | 139 | strcpy(ctext," CW"); |
8mona | 3:6474ab60854e | 140 | //ctext="CW"; |
8mona | 3:6474ab60854e | 141 | } |
8mona | 3:6474ab60854e | 142 | |
8mona | 5:2b9614aa1171 | 143 | |
8mona | 5:2b9614aa1171 | 144 | //down..stop |
8mona | 5:2b9614aa1171 | 145 | else if ( (2*WAIT_DELAY+UP_DURATION+DOWN_DURATION) <= d_Cycle.rem ) |
8mona | 2:ea066749e515 | 146 | { |
8mona | 3:6474ab60854e | 147 | //wait_ms(20); |
8mona | 5:2b9614aa1171 | 148 | int accel_index = d_Cycle.rem - ((2*WAIT_DELAY+UP_DURATION+DOWN_DURATION)); |
8mona | 4:83f7df775d46 | 149 | if (accel_index< DECEL_SIZE) |
8mona | 4:83f7df775d46 | 150 | { |
8mona | 5:2b9614aa1171 | 151 | motor_speed= -vol_decel[accel_index] * spd_table[sp_index]*donw_ratio; |
8mona | 4:83f7df775d46 | 152 | } |
8mona | 4:83f7df775d46 | 153 | else |
8mona | 4:83f7df775d46 | 154 | { |
8mona | 4:83f7df775d46 | 155 | motor_speed=0; |
8mona | 4:83f7df775d46 | 156 | } |
8mona | 2:ea066749e515 | 157 | strcpy(ctext,"OFF"); |
8mona | 2:ea066749e515 | 158 | } |
8mona | 2:ea066749e515 | 159 | |
8mona | 2:ea066749e515 | 160 | |
8mona | 2:ea066749e515 | 161 | if(d_Cycle.quot==TOTAL_TIMES) |
8mona | 2:ea066749e515 | 162 | { |
8mona | 2:ea066749e515 | 163 | timer_.detach(); |
8mona | 2:ea066749e515 | 164 | } |
8mona | 3:6474ab60854e | 165 | |
8mona | 2:ea066749e515 | 166 | |
8mona | 2:ea066749e515 | 167 | |
MikamiUitOpen | 1:d5e9bd9b38ad | 168 | /* |
MikamiUitOpen | 0:132eb47d57fb | 169 | char str[20]; |
8mona | 2:ea066749e515 | 170 | sprintf(str, "%d'%2d\"", msec.quot, msec.rem); |
MikamiUitOpen | 0:132eb47d57fb | 171 | lcd_.WriteStringXY(str, 0, 1); |
MikamiUitOpen | 1:d5e9bd9b38ad | 172 | */ |
8mona | 2:ea066749e515 | 173 | |
8mona | 2:ea066749e515 | 174 | //1 Row |
8mona | 2:ea066749e515 | 175 | //lcd_.WriteStringXY("#",0,0); |
8mona | 3:6474ab60854e | 176 | |
8mona | 5:2b9614aa1171 | 177 | |
8mona | 5:2b9614aa1171 | 178 | |
8mona | 5:2b9614aa1171 | 179 | lcd_.WriteValueXY("%5d/", d_Cycle.quot,0,0); |
8mona | 5:2b9614aa1171 | 180 | lcd_.WriteValue("%0dM",TOTAL_TIMES); |
8mona | 5:2b9614aa1171 | 181 | lcd_.WriteValue("%1.2f", motor_speed); |
8mona | 5:2b9614aa1171 | 182 | //lcd_.WriteValue("V%0d",motor_speed); |
8mona | 3:6474ab60854e | 183 | |
8mona | 2:ea066749e515 | 184 | |
8mona | 2:ea066749e515 | 185 | //2 Row |
8mona | 2:ea066749e515 | 186 | lcd_.WriteValueXY("%03dh", t_hr, 0, 1); |
8mona | 2:ea066749e515 | 187 | lcd_.WriteValue("%02dm", d_min.rem); |
8mona | 2:ea066749e515 | 188 | lcd_.WriteValue("%03ds/", d_sec.rem); |
8mona | 2:ea066749e515 | 189 | lcd_.WriteValue("%03dh", tf_hr); |
8mona | 2:ea066749e515 | 190 | lcd_.WriteValue("%02dm", df_min.rem); |
8mona | 3:6474ab60854e | 191 | |
8mona | 2:ea066749e515 | 192 | |
8mona | 5:2b9614aa1171 | 193 | |
8mona | 2:ea066749e515 | 194 | k++; |
MikamiUitOpen | 0:132eb47d57fb | 195 | } |
MikamiUitOpen | 0:132eb47d57fb | 196 | |
8mona | 2:ea066749e515 | 197 | |
8mona | 2:ea066749e515 | 198 | |
8mona | 2:ea066749e515 | 199 | void flip() { |
8mona | 2:ea066749e515 | 200 | static bool b = false; |
8mona | 4:83f7df775d46 | 201 | |
8mona | 2:ea066749e515 | 202 | if(b==false) |
8mona | 2:ea066749e515 | 203 | { |
8mona | 4:83f7df775d46 | 204 | timer_.attach(&TimerIsr, 0.05); |
8mona | 2:ea066749e515 | 205 | } |
8mona | 2:ea066749e515 | 206 | |
8mona | 2:ea066749e515 | 207 | else |
8mona | 2:ea066749e515 | 208 | { |
8mona | 2:ea066749e515 | 209 | timer_.detach(); |
8mona | 3:6474ab60854e | 210 | //Relay1=0; |
8mona | 3:6474ab60854e | 211 | sp_index++; |
8mona | 3:6474ab60854e | 212 | if (sp_index == 3) |
8mona | 3:6474ab60854e | 213 | { |
8mona | 3:6474ab60854e | 214 | sp_index = 0; |
8mona | 3:6474ab60854e | 215 | } |
8mona | 2:ea066749e515 | 216 | } |
8mona | 2:ea066749e515 | 217 | b=!b; |
8mona | 2:ea066749e515 | 218 | |
8mona | 2:ea066749e515 | 219 | } |
8mona | 2:ea066749e515 | 220 | |
8mona | 2:ea066749e515 | 221 | |
MikamiUitOpen | 0:132eb47d57fb | 222 | int main() |
MikamiUitOpen | 0:132eb47d57fb | 223 | { |
8mona | 5:2b9614aa1171 | 224 | |
8mona | 3:6474ab60854e | 225 | //motor.speed(0); |
8mona | 5:2b9614aa1171 | 226 | // Check error and reset |
8mona | 3:6474ab60854e | 227 | |
8mona | 2:ea066749e515 | 228 | //LCD_cont=0; |
8mona | 3:6474ab60854e | 229 | //if (lcd_.IsConnected()) printf("\r\nConnected"); |
8mona | 3:6474ab60854e | 230 | //else printf("\r\nDisconnected"); |
MikamiUitOpen | 0:132eb47d57fb | 231 | |
MikamiUitOpen | 0:132eb47d57fb | 232 | TimerIsr(); |
8mona | 2:ea066749e515 | 233 | //timer_.attach(&TimerIsr, 0.1); |
8mona | 2:ea066749e515 | 234 | button1.fall(&flip); |
MikamiUitOpen | 0:132eb47d57fb | 235 | |
8mona | 5:2b9614aa1171 | 236 | iqs62x.configure(); // configure the ICD |
8mona | 5:2b9614aa1171 | 237 | |
8mona | 5:2b9614aa1171 | 238 | |
8mona | 3:6474ab60854e | 239 | bool status = motor.status(); |
8mona | 3:6474ab60854e | 240 | if (status & DRV8830_F_FAULT){ |
8mona | 3:6474ab60854e | 241 | motor.reset(); |
8mona | 3:6474ab60854e | 242 | } |
8mona | 3:6474ab60854e | 243 | |
8mona | 3:6474ab60854e | 244 | while (true) { |
8mona | 3:6474ab60854e | 245 | motor.speed(motor_speed); |
8mona | 5:2b9614aa1171 | 246 | //iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers |
8mona | 5:2b9614aa1171 | 247 | //ShowLCD(iqs62x.registers,0x80,0x8f); |
8mona | 5:2b9614aa1171 | 248 | wait(1); |
8mona | 5:2b9614aa1171 | 249 | } |
8mona | 5:2b9614aa1171 | 250 | |
MikamiUitOpen | 0:132eb47d57fb | 251 | } |
MikamiUitOpen | 0:132eb47d57fb | 252 |