Use IQS62X sensor and move motor by detected angle

Dependencies:   DRV8830 IQS62x IQSDisplayTerminal UIT_ACM1602NI mbed

Fork of Nucleo_ACM1602_I2C_DC by Thinkbed

Committer:
8mona
Date:
Mon Oct 02 19:38:45 2017 +0000
Revision:
10:ef379cbc0004
Parent:
9:b58e7d72a91c
Child:
11:80b6c5d77073
re-define moving profile;

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
8mona 9:b58e7d72a91c 7 //#define ANGLE_ENABLE
MikamiUitOpen 0:132eb47d57fb 8 #include "ACM1602NI.hpp"
8mona 3:6474ab60854e 9 #include "DRV8830.h"
8mona 5:2b9614aa1171 10 #include "IQS62x.h"
8mona 5:2b9614aa1171 11 #include "IQSdisplayTerminal.h"
8mona 8:f7ad1d7176ba 12 #include "MotorMove.h"
8mona 3:6474ab60854e 13
8mona 2:ea066749e515 14 //Cycle
8mona 5:2b9614aa1171 15 #define UP_DURATION 14 //On time [*100ms]
8mona 5:2b9614aa1171 16 #define WAIT_DELAY 10 //Delay time [*100ms]
8mona 5:2b9614aa1171 17 #define DOWN_DURATION 8 //Down time [*50ms]
8mona 5:2b9614aa1171 18 #define SWITCH_PERIOD 50 //Cycle time[*50ms]
8mona 5:2b9614aa1171 19 #define TOTAL_TIMES 30000 //total times n
8mona 8:f7ad1d7176ba 20 #define TIMER_COUNT 0.01
8mona 9:b58e7d72a91c 21 #define LOOP_WAITMS 50
8mona 3:6474ab60854e 22
8mona 9:b58e7d72a91c 23 #define DEGREE_SHIFT 100
8mona 8:f7ad1d7176ba 24 #define UP_THRESHOLD 10
8mona 9:b58e7d72a91c 25 #define DOWN_THRESHOLD 30
8mona 3:6474ab60854e 26
8mona 5:2b9614aa1171 27
8mona 8:f7ad1d7176ba 28 Ticker timer_;
8mona 2:ea066749e515 29
MikamiUitOpen 0:132eb47d57fb 30 using namespace Mikami;
MikamiUitOpen 0:132eb47d57fb 31 Acm1602Ni lcd_; // Default, OK
MikamiUitOpen 0:132eb47d57fb 32 //Acm1602Ni lcd_(D14, D15); // OK
MikamiUitOpen 0:132eb47d57fb 33 //Acm1602Ni lcd_(D14, D15, 200000); // OK
MikamiUitOpen 0:132eb47d57fb 34 //Acm1602Ni lcd_(D14, D15, 200000, true, true); // OK
MikamiUitOpen 0:132eb47d57fb 35 //Acm1602Ni lcd_(PB_3, PB_10); // OK
MikamiUitOpen 0:132eb47d57fb 36 //Acm1602Ni lcd_(PC_9, PA_8); // OK
MikamiUitOpen 0:132eb47d57fb 37 //Acm1602Ni lcd_(PB_4, PA_8); // OK
MikamiUitOpen 0:132eb47d57fb 38
8mona 3:6474ab60854e 39 I2C i2c(D14, D15);
8mona 8:f7ad1d7176ba 40 MotorMove mvalL;
8mona 8:f7ad1d7176ba 41 MotorMove mvalR;
8mona 8:f7ad1d7176ba 42
8mona 9:b58e7d72a91c 43 #ifdef ANGLE_ENABLE
8mona 8:f7ad1d7176ba 44 IQS62xIO iqs62x; // class for basic IQS62x block read and write
8mona 9:b58e7d72a91c 45 #endif
8mona 9:b58e7d72a91c 46 DigitalIn button1(USER_BUTTON);
8mona 8:f7ad1d7176ba 47
8mona 8:f7ad1d7176ba 48
8mona 8:f7ad1d7176ba 49 DigitalIn in_switchs[]=
8mona 8:f7ad1d7176ba 50 { DigitalIn(A0), DigitalIn(A1), DigitalIn(A2), DigitalIn(A3)};
8mona 8:f7ad1d7176ba 51
8mona 8:f7ad1d7176ba 52
8mona 8:f7ad1d7176ba 53 static int shaft_deg=0;
8mona 8:f7ad1d7176ba 54 static int shaft_speed=0;
8mona 8:f7ad1d7176ba 55 static int g_timer=0; //gloabl timer
8mona 8:f7ad1d7176ba 56 static int cnt; //total timer of loop
8mona 8:f7ad1d7176ba 57 static int sw_in[4]={0,0,0,0}; //swithc flag bit
8mona 8:f7ad1d7176ba 58 static int sp_index=0; //Movement mode 0-3
8mona 8:f7ad1d7176ba 59
8mona 8:f7ad1d7176ba 60 static int initial_deg=0;
8mona 8:f7ad1d7176ba 61
8mona 9:b58e7d72a91c 62 DRV8830 motorL(i2c, DRV8830ADDR_NN); //Motor1
8mona 9:b58e7d72a91c 63 DRV8830 motorR(i2c, DRV8830ADDR_0N); //Motor2
8mona 9:b58e7d72a91c 64
8mona 8:f7ad1d7176ba 65
8mona 8:f7ad1d7176ba 66 void ShowLCD(char * buffer, int startbyte, int endbyte); // for wheel output
8mona 8:f7ad1d7176ba 67 int ReadDegree(char * buffer);
8mona 8:f7ad1d7176ba 68 int ReadSpeed(char * buffer);
8mona 8:f7ad1d7176ba 69 void Displaylevel (int deg);
8mona 8:f7ad1d7176ba 70 void TimerIsr();
8mona 8:f7ad1d7176ba 71 void flip();
8mona 8:f7ad1d7176ba 72 int MainIOloop();
8mona 8:f7ad1d7176ba 73 void DisplayStatus();
8mona 8:f7ad1d7176ba 74 void MoveMotor();
8mona 8:f7ad1d7176ba 75
8mona 8:f7ad1d7176ba 76 int main()
8mona 8:f7ad1d7176ba 77 {
8mona 8:f7ad1d7176ba 78 //initialize system
8mona 8:f7ad1d7176ba 79 static int time_prev;
8mona 8:f7ad1d7176ba 80 for (int i=0;i<4;i=i++){
8mona 8:f7ad1d7176ba 81 in_switchs[i].mode(PullUp);
8mona 8:f7ad1d7176ba 82 }
8mona 8:f7ad1d7176ba 83
8mona 8:f7ad1d7176ba 84 //motor.speed(0);
8mona 8:f7ad1d7176ba 85 //Initialize Ic2 Device
8mona 9:b58e7d72a91c 86
8mona 9:b58e7d72a91c 87
8mona 8:f7ad1d7176ba 88 motorL.speed(0);
8mona 8:f7ad1d7176ba 89 motorR.speed(0);
8mona 8:f7ad1d7176ba 90
8mona 9:b58e7d72a91c 91
8mona 9:b58e7d72a91c 92
8mona 9:b58e7d72a91c 93 #ifdef ANGLE_ENABLE
8mona 8:f7ad1d7176ba 94 lcd_.WriteStringXY("IQS_Init_Start",0,0);
8mona 8:f7ad1d7176ba 95 iqs62x.configure(); // configure
8mona 8:f7ad1d7176ba 96 wait(1);
8mona 8:f7ad1d7176ba 97 iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers
8mona 8:f7ad1d7176ba 98 initial_deg = ReadDegree(iqs62x.registers);
8mona 9:b58e7d72a91c 99 lcd_.WriteStringXY("IQS_Init_done ",0,0);
8mona 9:b58e7d72a91c 100 wait(1);
8mona 9:b58e7d72a91c 101 #else
8mona 9:b58e7d72a91c 102 lcd_.WriteStringXY("No_Sensor",0,0);
8mona 9:b58e7d72a91c 103 initial_deg=0;
8mona 9:b58e7d72a91c 104 #endif
8mona 9:b58e7d72a91c 105
8mona 9:b58e7d72a91c 106
8mona 8:f7ad1d7176ba 107
8mona 8:f7ad1d7176ba 108
8mona 8:f7ad1d7176ba 109
8mona 8:f7ad1d7176ba 110 //read 0deg for initialize
8mona 9:b58e7d72a91c 111 //button1.fall(&flip);
8mona 8:f7ad1d7176ba 112
8mona 8:f7ad1d7176ba 113 //TimerIsr();
8mona 8:f7ad1d7176ba 114 //timer_.attach(&TimerIsr, TIMER_COUNT);
8mona 8:f7ad1d7176ba 115 // bool status = motor.status();
8mona 8:f7ad1d7176ba 116 // if (status & DRV8830_F_FAULT){
8mona 8:f7ad1d7176ba 117 // motor.reset();
8mona 8:f7ad1d7176ba 118 // }
8mona 8:f7ad1d7176ba 119
8mona 8:f7ad1d7176ba 120 //Read here as Asynchronous when data gets ready
8mona 9:b58e7d72a91c 121 while (true) {
8mona 8:f7ad1d7176ba 122 int time_current = g_timer;
8mona 8:f7ad1d7176ba 123 int time_diff = time_current - time_prev;
8mona 8:f7ad1d7176ba 124 int a= MainIOloop();
8mona 8:f7ad1d7176ba 125 DisplayStatus();
8mona 8:f7ad1d7176ba 126 //display_info
8mona 8:f7ad1d7176ba 127 time_prev = time_current;
8mona 8:f7ad1d7176ba 128 //motorR.speed( (shaft_deg-180.0)/200.0 );
8mona 8:f7ad1d7176ba 129 wait_ms(LOOP_WAITMS);
8mona 8:f7ad1d7176ba 130 MoveMotor();
8mona 8:f7ad1d7176ba 131 cnt ++;
8mona 9:b58e7d72a91c 132 }
8mona 8:f7ad1d7176ba 133 }
8mona 8:f7ad1d7176ba 134
8mona 8:f7ad1d7176ba 135 void MoveMotor(){
8mona 8:f7ad1d7176ba 136 static int bflag_up_pre=0;
8mona 8:f7ad1d7176ba 137 static int bflag_down_pre=0;
8mona 8:f7ad1d7176ba 138 int bflag_up_cur =0;
8mona 8:f7ad1d7176ba 139 int bflag_down_cur =0;
8mona 8:f7ad1d7176ba 140 float lspeed;
8mona 8:f7ad1d7176ba 141 float rspeed;
8mona 8:f7ad1d7176ba 142
MikamiUitOpen 0:132eb47d57fb 143
8mona 9:b58e7d72a91c 144 //detect up or donw by thredold
8mona 9:b58e7d72a91c 145 #ifdef ANGLE_ENABLE
8mona 9:b58e7d72a91c 146 if (shaft_deg> (UP_THRESHOLD+DEGREE_SHIFT) )
8mona 8:f7ad1d7176ba 147 {
8mona 8:f7ad1d7176ba 148 bflag_up_cur=1;
8mona 8:f7ad1d7176ba 149 }
8mona 8:f7ad1d7176ba 150 else
8mona 8:f7ad1d7176ba 151 {
8mona 8:f7ad1d7176ba 152 bflag_up_cur=0;
8mona 8:f7ad1d7176ba 153 };
8mona 8:f7ad1d7176ba 154
8mona 9:b58e7d72a91c 155 if (shaft_deg< (DOWN_THRESHOLD+DEGREE_SHIFT) )
8mona 8:f7ad1d7176ba 156 {
8mona 8:f7ad1d7176ba 157 bflag_down_cur=1;
8mona 8:f7ad1d7176ba 158 }
8mona 8:f7ad1d7176ba 159 else
8mona 8:f7ad1d7176ba 160 {
8mona 8:f7ad1d7176ba 161 bflag_down_cur=0;
8mona 8:f7ad1d7176ba 162 };
8mona 9:b58e7d72a91c 163 #else
8mona 9:b58e7d72a91c 164 if (button1==1)
8mona 9:b58e7d72a91c 165 {
8mona 9:b58e7d72a91c 166 bflag_up_cur=1;
8mona 9:b58e7d72a91c 167 bflag_down_cur=0;
8mona 9:b58e7d72a91c 168 }
8mona 9:b58e7d72a91c 169 else
8mona 9:b58e7d72a91c 170 {
8mona 9:b58e7d72a91c 171 bflag_up_cur=0;
8mona 9:b58e7d72a91c 172 bflag_down_cur=1;
8mona 9:b58e7d72a91c 173 }
8mona 9:b58e7d72a91c 174 #endif
8mona 8:f7ad1d7176ba 175
8mona 8:f7ad1d7176ba 176 //send down or up command when status had changed
8mona 8:f7ad1d7176ba 177 if(bflag_up_pre==0&& bflag_up_cur==1)
8mona 8:f7ad1d7176ba 178 {
8mona 8:f7ad1d7176ba 179 //shaft_speed
8mona 9:b58e7d72a91c 180 mvalL.up_motor_set(cnt, 0.9);
8mona 9:b58e7d72a91c 181 mvalR.up_motor_set(cnt, 0.9);
8mona 8:f7ad1d7176ba 182 lcd_.WriteStringXY("U",0,1);
8mona 8:f7ad1d7176ba 183 }
8mona 9:b58e7d72a91c 184
8mona 8:f7ad1d7176ba 185 else if(bflag_down_pre==0 && bflag_down_cur==1)
8mona 8:f7ad1d7176ba 186 {
8mona 8:f7ad1d7176ba 187 mvalL.down_motor_set(cnt, 1.0);
8mona 8:f7ad1d7176ba 188 mvalR.down_motor_set(cnt, 1.0);
8mona 8:f7ad1d7176ba 189 lcd_.WriteStringXY("D",1,1);
8mona 9:b58e7d72a91c 190 }
8mona 8:f7ad1d7176ba 191 else{
8mona 8:f7ad1d7176ba 192 lcd_.WriteStringXY("__",0,1);
8mona 8:f7ad1d7176ba 193 }
8mona 8:f7ad1d7176ba 194
8mona 9:b58e7d72a91c 195 lspeed= mvalL.ReturnMotorVol(cnt, sw_in[0],sw_in[1]);
8mona 9:b58e7d72a91c 196 rspeed= -mvalR.ReturnMotorVol(cnt, sw_in[2],sw_in[3]);
8mona 8:f7ad1d7176ba 197
8mona 10:ef379cbc0004 198
8mona 8:f7ad1d7176ba 199 motorL.speed(lspeed);
8mona 10:ef379cbc0004 200 wait_ms(1);
8mona 8:f7ad1d7176ba 201 motorR.speed(rspeed);
8mona 9:b58e7d72a91c 202 //motorL.speed(0.5);
8mona 9:b58e7d72a91c 203 //motorR.speed(0.3);
8mona 8:f7ad1d7176ba 204
8mona 8:f7ad1d7176ba 205 lcd_.WriteValueXY("%1.2f",lspeed,3,1);
8mona 8:f7ad1d7176ba 206 lcd_.WriteValueXY("%1.2f",rspeed,8,1);
8mona 8:f7ad1d7176ba 207
8mona 8:f7ad1d7176ba 208
8mona 8:f7ad1d7176ba 209 bflag_up_pre = bflag_up_cur;
8mona 8:f7ad1d7176ba 210 bflag_down_pre = bflag_down_cur;
8mona 8:f7ad1d7176ba 211 }
8mona 8:f7ad1d7176ba 212
8mona 8:f7ad1d7176ba 213
8mona 5:2b9614aa1171 214
8mona 8:f7ad1d7176ba 215 void DisplayStatus()
8mona 8:f7ad1d7176ba 216 {
8mona 8:f7ad1d7176ba 217 lcd_.WriteValueXY("T%3d ",shaft_deg, 0,0);
8mona 8:f7ad1d7176ba 218 lcd_.WriteValue("V%2d",shaft_speed);
8mona 8:f7ad1d7176ba 219
8mona 8:f7ad1d7176ba 220 lcd_.WriteStringXY("F",9,0);
8mona 8:f7ad1d7176ba 221 for (int i=0;i<4;i++){
8mona 8:f7ad1d7176ba 222 lcd_.WriteValue("%d",sw_in[i]);
8mona 8:f7ad1d7176ba 223 }
8mona 8:f7ad1d7176ba 224 }
8mona 8:f7ad1d7176ba 225
8mona 8:f7ad1d7176ba 226 int MainIOloop()
8mona 8:f7ad1d7176ba 227 {
8mona 8:f7ad1d7176ba 228 static int cnt=0;
8mona 9:b58e7d72a91c 229 #ifdef ANGLE_ENABLE
8mona 9:b58e7d72a91c 230 iqs62x.waitForIqsReady();
8mona 8:f7ad1d7176ba 231 iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers
8mona 9:b58e7d72a91c 232 shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+ DEGREE_SHIFT;
8mona 9:b58e7d72a91c 233 #endif
8mona 9:b58e7d72a91c 234
8mona 9:b58e7d72a91c 235
8mona 8:f7ad1d7176ba 236 if(shaft_deg<0)
8mona 8:f7ad1d7176ba 237 {
8mona 9:b58e7d72a91c 238 shaft_deg = shaft_deg+360; // offset 100deg to cancel error
8mona 8:f7ad1d7176ba 239 }
8mona 8:f7ad1d7176ba 240
8mona 9:b58e7d72a91c 241
8mona 9:b58e7d72a91c 242 #ifdef ANGLE_ENABLE
8mona 8:f7ad1d7176ba 243 shaft_speed= ReadSpeed(iqs62x.registers);
8mona 8:f7ad1d7176ba 244 //lcd_.WriteValueXY("%3d ",k, 0,0);
8mona 9:b58e7d72a91c 245 #endif
8mona 8:f7ad1d7176ba 246
8mona 9:b58e7d72a91c 247 sw_in[0]= in_switchs[0];
8mona 9:b58e7d72a91c 248 sw_in[1]= !in_switchs[1];
8mona 9:b58e7d72a91c 249 sw_in[2]= in_switchs[2];
8mona 9:b58e7d72a91c 250 sw_in[3]= ! in_switchs[3];
8mona 9:b58e7d72a91c 251
8mona 9:b58e7d72a91c 252 /*
8mona 8:f7ad1d7176ba 253 for (int i=0;i<4;i=i++){
8mona 8:f7ad1d7176ba 254 sw_in[i]=!in_switchs[i];
8mona 9:b58e7d72a91c 255 sw_in[i+1]=!(in_switchs[i+1]);
8mona 8:f7ad1d7176ba 256 }
8mona 9:b58e7d72a91c 257 */
8mona 9:b58e7d72a91c 258
8mona 9:b58e7d72a91c 259
8mona 9:b58e7d72a91c 260
8mona 8:f7ad1d7176ba 261 cnt++;
8mona 9:b58e7d72a91c 262
8mona 9:b58e7d72a91c 263
8mona 9:b58e7d72a91c 264 bool statusL = motorL.status();
8mona 9:b58e7d72a91c 265 if (statusL & DRV8830_F_FAULT){
8mona 9:b58e7d72a91c 266 motorL.reset();
8mona 9:b58e7d72a91c 267 }
8mona 9:b58e7d72a91c 268
8mona 9:b58e7d72a91c 269 bool statusR = motorR.status();
8mona 9:b58e7d72a91c 270 if (statusR & DRV8830_F_FAULT){
8mona 9:b58e7d72a91c 271 motorR.reset();
8mona 9:b58e7d72a91c 272 }
8mona 9:b58e7d72a91c 273
8mona 9:b58e7d72a91c 274
8mona 9:b58e7d72a91c 275
8mona 8:f7ad1d7176ba 276 return cnt;
8mona 8:f7ad1d7176ba 277 }
8mona 8:f7ad1d7176ba 278
8mona 8:f7ad1d7176ba 279
8mona 8:f7ad1d7176ba 280 void TimerIsr()
8mona 8:f7ad1d7176ba 281 {
8mona 8:f7ad1d7176ba 282 //For LED Time-Sec display
8mona 8:f7ad1d7176ba 283 //wait_ms(5);
8mona 8:f7ad1d7176ba 284 g_timer++;
8mona 8:f7ad1d7176ba 285 //Displaylevel(val);
8mona 8:f7ad1d7176ba 286 }
8mona 8:f7ad1d7176ba 287
8mona 8:f7ad1d7176ba 288
8mona 8:f7ad1d7176ba 289 void flip() {
8mona 8:f7ad1d7176ba 290 static bool b = false;
8mona 8:f7ad1d7176ba 291
8mona 8:f7ad1d7176ba 292 if(b==false)
8mona 8:f7ad1d7176ba 293 {
8mona 8:f7ad1d7176ba 294 timer_.attach(&TimerIsr, TIMER_COUNT);
8mona 8:f7ad1d7176ba 295 }
8mona 8:f7ad1d7176ba 296
8mona 8:f7ad1d7176ba 297 else
8mona 8:f7ad1d7176ba 298 {
8mona 8:f7ad1d7176ba 299 timer_.detach();
8mona 8:f7ad1d7176ba 300 //Relay1=0;
8mona 8:f7ad1d7176ba 301 sp_index++;
8mona 8:f7ad1d7176ba 302 if (sp_index == 3)
8mona 8:f7ad1d7176ba 303 {
8mona 8:f7ad1d7176ba 304 sp_index = 0;
8mona 8:f7ad1d7176ba 305 }
8mona 8:f7ad1d7176ba 306 }
8mona 8:f7ad1d7176ba 307 b=!b;
8mona 8:f7ad1d7176ba 308
8mona 8:f7ad1d7176ba 309 }
8mona 5:2b9614aa1171 310
8mona 7:acb1074eaba6 311 int ReadDegree(char * buffer)
8mona 5:2b9614aa1171 312 {
8mona 7:acb1074eaba6 313 int ret=0;
8mona 7:acb1074eaba6 314 //(High bit + Low bit) * 360/65536
8mona 7:acb1074eaba6 315 //ret = ((buffer[0x80]<<8 + buffer[0x81])*0.00549316406 ;
8mona 7:acb1074eaba6 316 ret = (buffer[0x80]<<8 +buffer[0x81])/65536.0*360.0 ;
8mona 7:acb1074eaba6 317 return ret;
8mona 5:2b9614aa1171 318 }
8mona 5:2b9614aa1171 319
8mona 8:f7ad1d7176ba 320
8mona 7:acb1074eaba6 321 int ReadSpeed(char * buffer)
8mona 7:acb1074eaba6 322 {
8mona 7:acb1074eaba6 323 int ret=0;
8mona 7:acb1074eaba6 324 ret = (buffer[0x8E]);
8mona 7:acb1074eaba6 325 return ret;
8mona 7:acb1074eaba6 326 }
8mona 7:acb1074eaba6 327
8mona 7:acb1074eaba6 328 void Displaylevel (int deg)
8mona 7:acb1074eaba6 329 {
8mona 7:acb1074eaba6 330
8mona 7:acb1074eaba6 331 int level=deg>>5;
8mona 7:acb1074eaba6 332 lcd_.WriteStringXY("@",0,0);
8mona 7:acb1074eaba6 333 for (int i=0;i<12;i++)
8mona 7:acb1074eaba6 334 {
8mona 7:acb1074eaba6 335 if (i<level)
8mona 7:acb1074eaba6 336 {
8mona 7:acb1074eaba6 337 lcd_.WriteString("-");
8mona 7:acb1074eaba6 338 }
8mona 7:acb1074eaba6 339 else
8mona 7:acb1074eaba6 340 {
8mona 7:acb1074eaba6 341 lcd_.WriteString(" ");
8mona 7:acb1074eaba6 342 }
8mona 8:f7ad1d7176ba 343 }
MikamiUitOpen 0:132eb47d57fb 344 }
MikamiUitOpen 0:132eb47d57fb 345
8mona 2:ea066749e515 346
8mona 2:ea066749e515 347