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@9:b58e7d72a91c, 2017-10-02 (annotated)

Committer:
8mona
Date:
Mon Oct 02 19:12:02 2017 +0000
Revision:
9:b58e7d72a91c
Parent:
8:f7ad1d7176ba
Child:
10:ef379cbc0004
Enable non-sensor mode and define curve

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 8:f7ad1d7176ba 198 motorL.speed(lspeed);
8mona 8:f7ad1d7176ba 199 motorR.speed(rspeed);
8mona 9:b58e7d72a91c 200 //motorL.speed(0.5);
8mona 9:b58e7d72a91c 201 //motorR.speed(0.3);
8mona 8:f7ad1d7176ba 202
8mona 8:f7ad1d7176ba 203 lcd_.WriteValueXY("%1.2f",lspeed,3,1);
8mona 8:f7ad1d7176ba 204 lcd_.WriteValueXY("%1.2f",rspeed,8,1);
8mona 8:f7ad1d7176ba 205
8mona 8:f7ad1d7176ba 206
8mona 8:f7ad1d7176ba 207 bflag_up_pre = bflag_up_cur;
8mona 8:f7ad1d7176ba 208 bflag_down_pre = bflag_down_cur;
8mona 8:f7ad1d7176ba 209 }
8mona 8:f7ad1d7176ba 210
8mona 8:f7ad1d7176ba 211
8mona 5:2b9614aa1171 212
8mona 8:f7ad1d7176ba 213 void DisplayStatus()
8mona 8:f7ad1d7176ba 214 {
8mona 8:f7ad1d7176ba 215 lcd_.WriteValueXY("T%3d ",shaft_deg, 0,0);
8mona 8:f7ad1d7176ba 216 lcd_.WriteValue("V%2d",shaft_speed);
8mona 8:f7ad1d7176ba 217
8mona 8:f7ad1d7176ba 218 lcd_.WriteStringXY("F",9,0);
8mona 8:f7ad1d7176ba 219 for (int i=0;i<4;i++){
8mona 8:f7ad1d7176ba 220 lcd_.WriteValue("%d",sw_in[i]);
8mona 8:f7ad1d7176ba 221 }
8mona 8:f7ad1d7176ba 222 }
8mona 8:f7ad1d7176ba 223
8mona 8:f7ad1d7176ba 224 int MainIOloop()
8mona 8:f7ad1d7176ba 225 {
8mona 8:f7ad1d7176ba 226 static int cnt=0;
8mona 9:b58e7d72a91c 227 #ifdef ANGLE_ENABLE
8mona 9:b58e7d72a91c 228 iqs62x.waitForIqsReady();
8mona 8:f7ad1d7176ba 229 iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers
8mona 9:b58e7d72a91c 230 shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+ DEGREE_SHIFT;
8mona 9:b58e7d72a91c 231 #endif
8mona 9:b58e7d72a91c 232
8mona 9:b58e7d72a91c 233
8mona 8:f7ad1d7176ba 234 if(shaft_deg<0)
8mona 8:f7ad1d7176ba 235 {
8mona 9:b58e7d72a91c 236 shaft_deg = shaft_deg+360; // offset 100deg to cancel error
8mona 8:f7ad1d7176ba 237 }
8mona 8:f7ad1d7176ba 238
8mona 9:b58e7d72a91c 239
8mona 9:b58e7d72a91c 240 #ifdef ANGLE_ENABLE
8mona 8:f7ad1d7176ba 241 shaft_speed= ReadSpeed(iqs62x.registers);
8mona 8:f7ad1d7176ba 242 //lcd_.WriteValueXY("%3d ",k, 0,0);
8mona 9:b58e7d72a91c 243 #endif
8mona 8:f7ad1d7176ba 244
8mona 9:b58e7d72a91c 245 sw_in[0]= in_switchs[0];
8mona 9:b58e7d72a91c 246 sw_in[1]= !in_switchs[1];
8mona 9:b58e7d72a91c 247 sw_in[2]= in_switchs[2];
8mona 9:b58e7d72a91c 248 sw_in[3]= ! in_switchs[3];
8mona 9:b58e7d72a91c 249
8mona 9:b58e7d72a91c 250 /*
8mona 8:f7ad1d7176ba 251 for (int i=0;i<4;i=i++){
8mona 8:f7ad1d7176ba 252 sw_in[i]=!in_switchs[i];
8mona 9:b58e7d72a91c 253 sw_in[i+1]=!(in_switchs[i+1]);
8mona 8:f7ad1d7176ba 254 }
8mona 9:b58e7d72a91c 255 */
8mona 9:b58e7d72a91c 256
8mona 9:b58e7d72a91c 257
8mona 9:b58e7d72a91c 258
8mona 8:f7ad1d7176ba 259 cnt++;
8mona 9:b58e7d72a91c 260
8mona 9:b58e7d72a91c 261
8mona 9:b58e7d72a91c 262 bool statusL = motorL.status();
8mona 9:b58e7d72a91c 263 if (statusL & DRV8830_F_FAULT){
8mona 9:b58e7d72a91c 264 motorL.reset();
8mona 9:b58e7d72a91c 265 }
8mona 9:b58e7d72a91c 266
8mona 9:b58e7d72a91c 267 bool statusR = motorR.status();
8mona 9:b58e7d72a91c 268 if (statusR & DRV8830_F_FAULT){
8mona 9:b58e7d72a91c 269 motorR.reset();
8mona 9:b58e7d72a91c 270 }
8mona 9:b58e7d72a91c 271
8mona 9:b58e7d72a91c 272
8mona 9:b58e7d72a91c 273
8mona 8:f7ad1d7176ba 274 return cnt;
8mona 8:f7ad1d7176ba 275 }
8mona 8:f7ad1d7176ba 276
8mona 8:f7ad1d7176ba 277
8mona 8:f7ad1d7176ba 278 void TimerIsr()
8mona 8:f7ad1d7176ba 279 {
8mona 8:f7ad1d7176ba 280 //For LED Time-Sec display
8mona 8:f7ad1d7176ba 281 //wait_ms(5);
8mona 8:f7ad1d7176ba 282 g_timer++;
8mona 8:f7ad1d7176ba 283 //Displaylevel(val);
8mona 8:f7ad1d7176ba 284 }
8mona 8:f7ad1d7176ba 285
8mona 8:f7ad1d7176ba 286
8mona 8:f7ad1d7176ba 287 void flip() {
8mona 8:f7ad1d7176ba 288 static bool b = false;
8mona 8:f7ad1d7176ba 289
8mona 8:f7ad1d7176ba 290 if(b==false)
8mona 8:f7ad1d7176ba 291 {
8mona 8:f7ad1d7176ba 292 timer_.attach(&TimerIsr, TIMER_COUNT);
8mona 8:f7ad1d7176ba 293 }
8mona 8:f7ad1d7176ba 294
8mona 8:f7ad1d7176ba 295 else
8mona 8:f7ad1d7176ba 296 {
8mona 8:f7ad1d7176ba 297 timer_.detach();
8mona 8:f7ad1d7176ba 298 //Relay1=0;
8mona 8:f7ad1d7176ba 299 sp_index++;
8mona 8:f7ad1d7176ba 300 if (sp_index == 3)
8mona 8:f7ad1d7176ba 301 {
8mona 8:f7ad1d7176ba 302 sp_index = 0;
8mona 8:f7ad1d7176ba 303 }
8mona 8:f7ad1d7176ba 304 }
8mona 8:f7ad1d7176ba 305 b=!b;
8mona 8:f7ad1d7176ba 306
8mona 8:f7ad1d7176ba 307 }
8mona 5:2b9614aa1171 308
8mona 7:acb1074eaba6 309 int ReadDegree(char * buffer)
8mona 5:2b9614aa1171 310 {
8mona 7:acb1074eaba6 311 int ret=0;
8mona 7:acb1074eaba6 312 //(High bit + Low bit) * 360/65536
8mona 7:acb1074eaba6 313 //ret = ((buffer[0x80]<<8 + buffer[0x81])*0.00549316406 ;
8mona 7:acb1074eaba6 314 ret = (buffer[0x80]<<8 +buffer[0x81])/65536.0*360.0 ;
8mona 7:acb1074eaba6 315 return ret;
8mona 5:2b9614aa1171 316 }
8mona 5:2b9614aa1171 317
8mona 8:f7ad1d7176ba 318
8mona 7:acb1074eaba6 319 int ReadSpeed(char * buffer)
8mona 7:acb1074eaba6 320 {
8mona 7:acb1074eaba6 321 int ret=0;
8mona 7:acb1074eaba6 322 ret = (buffer[0x8E]);
8mona 7:acb1074eaba6 323 return ret;
8mona 7:acb1074eaba6 324 }
8mona 7:acb1074eaba6 325
8mona 7:acb1074eaba6 326 void Displaylevel (int deg)
8mona 7:acb1074eaba6 327 {
8mona 7:acb1074eaba6 328
8mona 7:acb1074eaba6 329 int level=deg>>5;
8mona 7:acb1074eaba6 330 lcd_.WriteStringXY("@",0,0);
8mona 7:acb1074eaba6 331 for (int i=0;i<12;i++)
8mona 7:acb1074eaba6 332 {
8mona 7:acb1074eaba6 333 if (i<level)
8mona 7:acb1074eaba6 334 {
8mona 7:acb1074eaba6 335 lcd_.WriteString("-");
8mona 7:acb1074eaba6 336 }
8mona 7:acb1074eaba6 337 else
8mona 7:acb1074eaba6 338 {
8mona 7:acb1074eaba6 339 lcd_.WriteString(" ");
8mona 7:acb1074eaba6 340 }
8mona 8:f7ad1d7176ba 341 }
MikamiUitOpen 0:132eb47d57fb 342 }
MikamiUitOpen 0:132eb47d57fb 343
8mona 2:ea066749e515 344
8mona 2:ea066749e515 345