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@8:f7ad1d7176ba, 2017-09-30 (annotated)

Committer:
8mona
Date:
Sat Sep 30 16:43:51 2017 +0000
Revision:
8:f7ad1d7176ba
Parent:
7:acb1074eaba6
Child:
9:b58e7d72a91c
Activate IQS62X and motor and IO Switch

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