Thinkbed / Mbed 2 deprecated Nucleo_ACM1602_I2C_DC_Angle

Use IQS62X sensor and move motor by detected angle

Dependencies:   DRV8830 IQS62x IQSDisplayTerminal UIT_ACM1602NI mbed

Fork of Nucleo_ACM1602_I2C_DC by Thinkbed

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?

UserRevisionLine numberNew 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