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@12:8464be95bf76, 2018-01-30 (annotated)

Committer:
8mona
Date:
Tue Jan 30 06:55:27 2018 +0000
Revision:
12:8464be95bf76
Parent:
11:80b6c5d77073 
Up version

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 12:8464be95bf76 7
8mona 12:8464be95bf76 8 //For Flag if we need sample or not
8mona 9:b58e7d72a91c 9 //#define ANGLE_ENABLE
8mona 12:8464be95bf76 10 #define SERIAL_ENABLE
8mona 12:8464be95bf76 11 //#define LED_ENABLE
8mona 12:8464be95bf76 12
MikamiUitOpen 0:132eb47d57fb 13 #include "ACM1602NI.hpp"
8mona 3:6474ab60854e 14 #include "DRV8830.h"
8mona 5:2b9614aa1171 15 #include "IQS62x.h"
8mona 5:2b9614aa1171 16 #include "IQSdisplayTerminal.h"
8mona 8:f7ad1d7176ba 17 #include "MotorMove.h"
8mona 3:6474ab60854e 18
8mona 2:ea066749e515 19 //Cycle
8mona 5:2b9614aa1171 20 #define UP_DURATION 14 //On time [*100ms]
8mona 5:2b9614aa1171 21 #define WAIT_DELAY 10 //Delay time [*100ms]
8mona 5:2b9614aa1171 22 #define DOWN_DURATION 8 //Down time [*50ms]
8mona 5:2b9614aa1171 23 #define SWITCH_PERIOD 50 //Cycle time[*50ms]
8mona 5:2b9614aa1171 24 #define TOTAL_TIMES 30000 //total times n
8mona 8:f7ad1d7176ba 25 #define TIMER_COUNT 0.01
8mona 12:8464be95bf76 26 #define MIN_ANGLE -90
8mona 12:8464be95bf76 27
8mona 3:6474ab60854e 28
8mona 12:8464be95bf76 29 #define LOOP_WAITMS 50
8mona 12:8464be95bf76 30 #define SPEED_RATIO 0.5
8mona 3:6474ab60854e 31
8mona 12:8464be95bf76 32 #define UP_THRESHOLD 80
8mona 12:8464be95bf76 33 #define DOWN_THRESHOLD 70
8mona 11:80b6c5d77073 34
8mona 11:80b6c5d77073 35
8mona 12:8464be95bf76 36 #define NUM_LED 3
8mona 12:8464be95bf76 37 #define MAX_LED_ANGLE 100
8mona 12:8464be95bf76 38 #define MAX_LED_BRI 0.7
8mona 12:8464be95bf76 39 #define HSV_SHIFT 0.0
8mona 12:8464be95bf76 40
8mona 12:8464be95bf76 41 /*
8mona 12:8464be95bf76 42 #define L_UP .5
8mona 12:8464be95bf76 43 #define R_UP .5
8mona 12:8464be95bf76 44 #define L_DOWN .5
8mona 12:8464be95bf76 45 #define R_DOWN .5
8mona 12:8464be95bf76 46 */
8mona 12:8464be95bf76 47
8mona 12:8464be95bf76 48
8mona 12:8464be95bf76 49 #define L_UP 0.7
8mona 12:8464be95bf76 50 #define R_UP 0.7
8mona 12:8464be95bf76 51 #define L_DOWN 0.65
8mona 12:8464be95bf76 52 #define R_DOWN 0.65
8mona 11:80b6c5d77073 53
8mona 11:80b6c5d77073 54
8mona 5:2b9614aa1171 55
8mona 8:f7ad1d7176ba 56 Ticker timer_;
8mona 2:ea066749e515 57
MikamiUitOpen 0:132eb47d57fb 58 using namespace Mikami;
MikamiUitOpen 0:132eb47d57fb 59 Acm1602Ni lcd_; // Default, OK
MikamiUitOpen 0:132eb47d57fb 60 //Acm1602Ni lcd_(D14, D15); // OK
MikamiUitOpen 0:132eb47d57fb 61 //Acm1602Ni lcd_(D14, D15, 200000); // OK
MikamiUitOpen 0:132eb47d57fb 62 //Acm1602Ni lcd_(D14, D15, 200000, true, true); // OK
MikamiUitOpen 0:132eb47d57fb 63 //Acm1602Ni lcd_(PB_3, PB_10); // OK
MikamiUitOpen 0:132eb47d57fb 64 //Acm1602Ni lcd_(PC_9, PA_8); // OK
MikamiUitOpen 0:132eb47d57fb 65 //Acm1602Ni lcd_(PB_4, PA_8); // OK
MikamiUitOpen 0:132eb47d57fb 66
8mona 3:6474ab60854e 67 I2C i2c(D14, D15);
8mona 8:f7ad1d7176ba 68 MotorMove mvalL;
8mona 8:f7ad1d7176ba 69 MotorMove mvalR;
8mona 8:f7ad1d7176ba 70
8mona 12:8464be95bf76 71 #ifdef SERIAL_ENABLE
8mona 12:8464be95bf76 72 Serial pc(USBTX, USBRX); // tx, rx
8mona 12:8464be95bf76 73 #endif
8mona 12:8464be95bf76 74
8mona 9:b58e7d72a91c 75 #ifdef ANGLE_ENABLE
8mona 8:f7ad1d7176ba 76 IQS62xIO iqs62x; // class for basic IQS62x block read and write
8mona 9:b58e7d72a91c 77 #endif
8mona 9:b58e7d72a91c 78 DigitalIn button1(USER_BUTTON);
8mona 8:f7ad1d7176ba 79
8mona 8:f7ad1d7176ba 80
8mona 12:8464be95bf76 81 #ifdef LED_ENABLE
8mona 12:8464be95bf76 82 PwmOut LEDPWM[NUM_LED]= {D3,D6,D9};
8mona 12:8464be95bf76 83 #endif
8mona 12:8464be95bf76 84
8mona 8:f7ad1d7176ba 85 DigitalIn in_switchs[]=
8mona 8:f7ad1d7176ba 86 { DigitalIn(A0), DigitalIn(A1), DigitalIn(A2), DigitalIn(A3)};
8mona 8:f7ad1d7176ba 87
8mona 8:f7ad1d7176ba 88 static int shaft_deg=0;
8mona 8:f7ad1d7176ba 89 static int shaft_speed=0;
8mona 8:f7ad1d7176ba 90 static int g_timer=0; //gloabl timer
8mona 8:f7ad1d7176ba 91 static int cnt; //total timer of loop
8mona 8:f7ad1d7176ba 92 static int sw_in[4]={0,0,0,0}; //swithc flag bit
8mona 8:f7ad1d7176ba 93 static int sp_index=0; //Movement mode 0-3
8mona 8:f7ad1d7176ba 94
8mona 8:f7ad1d7176ba 95 static int initial_deg=0;
8mona 12:8464be95bf76 96 static int degree_shift=0;
8mona 12:8464be95bf76 97
8mona 12:8464be95bf76 98 float pwmPeriod=0.000005;
8mona 8:f7ad1d7176ba 99
8mona 9:b58e7d72a91c 100 DRV8830 motorL(i2c, DRV8830ADDR_NN); //Motor1
8mona 9:b58e7d72a91c 101 DRV8830 motorR(i2c, DRV8830ADDR_0N); //Motor2
8mona 9:b58e7d72a91c 102
8mona 8:f7ad1d7176ba 103
8mona 8:f7ad1d7176ba 104 void ShowLCD(char * buffer, int startbyte, int endbyte); // for wheel output
8mona 8:f7ad1d7176ba 105 int ReadDegree(char * buffer);
8mona 8:f7ad1d7176ba 106 int ReadSpeed(char * buffer);
8mona 8:f7ad1d7176ba 107 void Displaylevel (int deg);
8mona 8:f7ad1d7176ba 108 void TimerIsr();
8mona 8:f7ad1d7176ba 109 void flip();
8mona 8:f7ad1d7176ba 110 int MainIOloop();
8mona 8:f7ad1d7176ba 111 void DisplayStatus();
8mona 8:f7ad1d7176ba 112 void MoveMotor();
8mona 12:8464be95bf76 113 void ShowLED(int angle);
8mona 12:8464be95bf76 114 void HSV2RGB(const float *HSV, float *RGB);
8mona 12:8464be95bf76 115
8mona 12:8464be95bf76 116
8mona 12:8464be95bf76 117
8mona 12:8464be95bf76 118
8mona 8:f7ad1d7176ba 119
8mona 8:f7ad1d7176ba 120 int main()
8mona 8:f7ad1d7176ba 121 {
8mona 8:f7ad1d7176ba 122 //initialize system
8mona 8:f7ad1d7176ba 123 static int time_prev;
8mona 8:f7ad1d7176ba 124 for (int i=0;i<4;i=i++){
8mona 8:f7ad1d7176ba 125 in_switchs[i].mode(PullUp);
8mona 8:f7ad1d7176ba 126 }
8mona 8:f7ad1d7176ba 127
8mona 8:f7ad1d7176ba 128 //motor.speed(0);
8mona 8:f7ad1d7176ba 129 //Initialize Ic2 Device
8mona 9:b58e7d72a91c 130
8mona 9:b58e7d72a91c 131
8mona 12:8464be95bf76 132 //motorL.speed(0);
8mona 12:8464be95bf76 133 //motorR.speed(0);
8mona 8:f7ad1d7176ba 134
8mona 9:b58e7d72a91c 135
8mona 9:b58e7d72a91c 136
8mona 9:b58e7d72a91c 137 #ifdef ANGLE_ENABLE
8mona 12:8464be95bf76 138 lcd_.WriteStringXY("IQS_Calibration",0,0);
8mona 12:8464be95bf76 139 //wait(1);
8mona 12:8464be95bf76 140 //iqs62x.configure(); // configure
8mona 12:8464be95bf76 141 //wait(1);
8mona 12:8464be95bf76 142 lcd_.WriteStringXY("IQS_Cconfig done",0,0);
8mona 12:8464be95bf76 143
8mona 12:8464be95bf76 144 iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers
8mona 12:8464be95bf76 145 initial_deg = ReadDegree(iqs62x.registers);
8mona 12:8464be95bf76 146
8mona 12:8464be95bf76 147 //degree_shift = (360- ReadDegree(iqs62x.registers) );
8mona 12:8464be95bf76 148
8mona 12:8464be95bf76 149
8mona 12:8464be95bf76 150 #ifdef LED_ENABLE
8mona 12:8464be95bf76 151 for (int i=0; i<NUM_LED ;i=i+1)
8mona 12:8464be95bf76 152 {
8mona 12:8464be95bf76 153 LEDPWM[i].period(pwmPeriod);
8mona 12:8464be95bf76 154 LEDPWM[i]=1;
8mona 12:8464be95bf76 155 }
8mona 12:8464be95bf76 156 #endif
8mona 12:8464be95bf76 157
8mona 8:f7ad1d7176ba 158 wait(1);
8mona 9:b58e7d72a91c 159 lcd_.WriteStringXY("IQS_Init_done ",0,0);
8mona 9:b58e7d72a91c 160 #else
8mona 9:b58e7d72a91c 161 lcd_.WriteStringXY("No_Sensor",0,0);
8mona 9:b58e7d72a91c 162 initial_deg=0;
8mona 9:b58e7d72a91c 163 #endif
8mona 9:b58e7d72a91c 164
8mona 12:8464be95bf76 165
8mona 8:f7ad1d7176ba 166
8mona 8:f7ad1d7176ba 167
8mona 8:f7ad1d7176ba 168
8mona 8:f7ad1d7176ba 169 //read 0deg for initialize
8mona 9:b58e7d72a91c 170 //button1.fall(&flip);
8mona 8:f7ad1d7176ba 171
8mona 8:f7ad1d7176ba 172 //TimerIsr();
8mona 8:f7ad1d7176ba 173 //timer_.attach(&TimerIsr, TIMER_COUNT);
8mona 8:f7ad1d7176ba 174 // bool status = motor.status();
8mona 8:f7ad1d7176ba 175 // if (status & DRV8830_F_FAULT){
8mona 8:f7ad1d7176ba 176 // motor.reset();
8mona 8:f7ad1d7176ba 177 // }
8mona 8:f7ad1d7176ba 178
8mona 8:f7ad1d7176ba 179 //Read here as Asynchronous when data gets ready
8mona 9:b58e7d72a91c 180 while (true) {
8mona 8:f7ad1d7176ba 181 int time_current = g_timer;
8mona 8:f7ad1d7176ba 182 int time_diff = time_current - time_prev;
8mona 8:f7ad1d7176ba 183 int a= MainIOloop();
8mona 8:f7ad1d7176ba 184 DisplayStatus();
8mona 8:f7ad1d7176ba 185 //display_info
8mona 8:f7ad1d7176ba 186 time_prev = time_current;
8mona 8:f7ad1d7176ba 187 //motorR.speed( (shaft_deg-180.0)/200.0 );
8mona 8:f7ad1d7176ba 188 wait_ms(LOOP_WAITMS);
8mona 8:f7ad1d7176ba 189 MoveMotor();
8mona 12:8464be95bf76 190
8mona 12:8464be95bf76 191
8mona 12:8464be95bf76 192 #ifdef SERIAL_ENABLE
8mona 12:8464be95bf76 193 pc.printf("%d\r\n", shaft_deg);
8mona 12:8464be95bf76 194 #endif
8mona 12:8464be95bf76 195
8mona 12:8464be95bf76 196
8mona 12:8464be95bf76 197 /*
8mona 12:8464be95bf76 198 LEDPWM[0]=1;
8mona 12:8464be95bf76 199 LEDPWM[1]=1;
8mona 12:8464be95bf76 200 LEDPWM[2]=1;
8mona 12:8464be95bf76 201 */
8mona 8:f7ad1d7176ba 202 cnt ++;
8mona 9:b58e7d72a91c 203 }
8mona 8:f7ad1d7176ba 204 }
8mona 8:f7ad1d7176ba 205
8mona 8:f7ad1d7176ba 206 void MoveMotor(){
8mona 8:f7ad1d7176ba 207 static int bflag_up_pre=0;
8mona 8:f7ad1d7176ba 208 static int bflag_down_pre=0;
8mona 8:f7ad1d7176ba 209 int bflag_up_cur =0;
8mona 8:f7ad1d7176ba 210 int bflag_down_cur =0;
8mona 8:f7ad1d7176ba 211 float lspeed;
8mona 8:f7ad1d7176ba 212 float rspeed;
8mona 8:f7ad1d7176ba 213
MikamiUitOpen 0:132eb47d57fb 214
8mona 9:b58e7d72a91c 215 //detect up or donw by thredold
8mona 9:b58e7d72a91c 216 #ifdef ANGLE_ENABLE
8mona 12:8464be95bf76 217
8mona 12:8464be95bf76 218
8mona 12:8464be95bf76 219
8mona 12:8464be95bf76 220 if( button1==1)
8mona 12:8464be95bf76 221 {
8mona 12:8464be95bf76 222 if (shaft_deg> (UP_THRESHOLD+degree_shift))
8mona 12:8464be95bf76 223 {
8mona 12:8464be95bf76 224 bflag_up_cur=1;
8mona 12:8464be95bf76 225 }
8mona 12:8464be95bf76 226 else
8mona 12:8464be95bf76 227 {
8mona 12:8464be95bf76 228 bflag_up_cur=0;
8mona 12:8464be95bf76 229 };
8mona 12:8464be95bf76 230
8mona 8:f7ad1d7176ba 231
8mona 12:8464be95bf76 232 if (shaft_deg< (DOWN_THRESHOLD+degree_shift) )
8mona 12:8464be95bf76 233 {
8mona 12:8464be95bf76 234 bflag_down_cur=1;
8mona 12:8464be95bf76 235 }
8mona 12:8464be95bf76 236 else
8mona 12:8464be95bf76 237 {
8mona 12:8464be95bf76 238 bflag_down_cur=0;
8mona 12:8464be95bf76 239 }
8mona 12:8464be95bf76 240
8mona 12:8464be95bf76 241 }
8mona 12:8464be95bf76 242
8mona 12:8464be95bf76 243
8mona 12:8464be95bf76 244
8mona 12:8464be95bf76 245 else if (button1==0)
8mona 8:f7ad1d7176ba 246 {
8mona 12:8464be95bf76 247 bflag_up_cur=0;
8mona 12:8464be95bf76 248 bflag_down_cur=1;
8mona 12:8464be95bf76 249 }
8mona 12:8464be95bf76 250
8mona 12:8464be95bf76 251
8mona 12:8464be95bf76 252
8mona 12:8464be95bf76 253
8mona 12:8464be95bf76 254
8mona 9:b58e7d72a91c 255 #else
8mona 9:b58e7d72a91c 256 if (button1==1)
8mona 9:b58e7d72a91c 257 {
8mona 9:b58e7d72a91c 258 bflag_up_cur=1;
8mona 9:b58e7d72a91c 259 bflag_down_cur=0;
8mona 9:b58e7d72a91c 260 }
8mona 9:b58e7d72a91c 261 else
8mona 9:b58e7d72a91c 262 {
8mona 9:b58e7d72a91c 263 bflag_up_cur=0;
8mona 9:b58e7d72a91c 264 bflag_down_cur=1;
8mona 9:b58e7d72a91c 265 }
8mona 9:b58e7d72a91c 266 #endif
8mona 8:f7ad1d7176ba 267
8mona 12:8464be95bf76 268
8mona 12:8464be95bf76 269 int mot_speed;
8mona 12:8464be95bf76 270
8mona 12:8464be95bf76 271 if (shaft_speed==0)
8mona 12:8464be95bf76 272 {
8mona 12:8464be95bf76 273 mot_speed=1;
8mona 12:8464be95bf76 274 }
8mona 12:8464be95bf76 275 else
8mona 12:8464be95bf76 276 {
8mona 12:8464be95bf76 277 mot_speed = shaft_speed;
8mona 12:8464be95bf76 278 }
8mona 12:8464be95bf76 279
8mona 12:8464be95bf76 280
8mona 12:8464be95bf76 281
8mona 8:f7ad1d7176ba 282 //send down or up command when status had changed
8mona 8:f7ad1d7176ba 283 if(bflag_up_pre==0&& bflag_up_cur==1)
8mona 8:f7ad1d7176ba 284 {
8mona 8:f7ad1d7176ba 285 //shaft_speed
8mona 12:8464be95bf76 286 // mvalL.up_motor_set(cnt, L_UP * mot_speed * SPEED_RATIO);
8mona 12:8464be95bf76 287 // mvalR.up_motor_set(cnt, R_UP * mot_speed * SPEED_RATIO);
8mona 11:80b6c5d77073 288 mvalL.up_motor_set(cnt, L_UP);
8mona 11:80b6c5d77073 289 mvalR.up_motor_set(cnt, R_UP);
8mona 8:f7ad1d7176ba 290 lcd_.WriteStringXY("U",0,1);
8mona 8:f7ad1d7176ba 291 }
8mona 9:b58e7d72a91c 292
8mona 8:f7ad1d7176ba 293 else if(bflag_down_pre==0 && bflag_down_cur==1)
8mona 8:f7ad1d7176ba 294 {
8mona 12:8464be95bf76 295 // mvalL.down_motor_set(cnt, L_DOWN * mot_speed *SPEED_RATIO);
8mona 12:8464be95bf76 296 // mvalR.down_motor_set(cnt, R_DOWN * mot_speed *SPEED_RATIO );
8mona 11:80b6c5d77073 297 mvalL.down_motor_set(cnt, L_DOWN);
8mona 11:80b6c5d77073 298 mvalR.down_motor_set(cnt, R_DOWN);
8mona 8:f7ad1d7176ba 299 lcd_.WriteStringXY("D",1,1);
8mona 9:b58e7d72a91c 300 }
8mona 8:f7ad1d7176ba 301 else{
8mona 8:f7ad1d7176ba 302 lcd_.WriteStringXY("__",0,1);
8mona 8:f7ad1d7176ba 303 }
8mona 8:f7ad1d7176ba 304
8mona 9:b58e7d72a91c 305 lspeed= mvalL.ReturnMotorVol(cnt, sw_in[0],sw_in[1]);
8mona 9:b58e7d72a91c 306 rspeed= -mvalR.ReturnMotorVol(cnt, sw_in[2],sw_in[3]);
8mona 8:f7ad1d7176ba 307
8mona 10:ef379cbc0004 308
8mona 8:f7ad1d7176ba 309 motorL.speed(lspeed);
8mona 10:ef379cbc0004 310 wait_ms(1);
8mona 8:f7ad1d7176ba 311 motorR.speed(rspeed);
8mona 9:b58e7d72a91c 312 //motorL.speed(0.5);
8mona 9:b58e7d72a91c 313 //motorR.speed(0.3);
8mona 8:f7ad1d7176ba 314
8mona 12:8464be95bf76 315 lcd_.WriteValueXY("%1.2f ",lspeed*5,3,1);
8mona 12:8464be95bf76 316 lcd_.WriteValueXY("%1.2f ",rspeed*5,8,1);
8mona 8:f7ad1d7176ba 317
8mona 8:f7ad1d7176ba 318
8mona 8:f7ad1d7176ba 319 bflag_up_pre = bflag_up_cur;
8mona 8:f7ad1d7176ba 320 bflag_down_pre = bflag_down_cur;
8mona 8:f7ad1d7176ba 321 }
8mona 8:f7ad1d7176ba 322
8mona 12:8464be95bf76 323
8mona 12:8464be95bf76 324
8mona 12:8464be95bf76 325 #ifdef LED_ENABLE
8mona 12:8464be95bf76 326 void ShowLED(int angle)
8mona 12:8464be95bf76 327 {
8mona 12:8464be95bf76 328 float led_H;
8mona 12:8464be95bf76 329 float led_V;
8mona 12:8464be95bf76 330
8mona 12:8464be95bf76 331 led_H = (float)angle/(float)MAX_LED_ANGLE+HSV_SHIFT;
8mona 12:8464be95bf76 332 //led_val = (float)angle/(float)MAX_LED_ANGLE;
8mona 12:8464be95bf76 333 led_V = (float)angle/(float)MAX_LED_ANGLE;
8mona 12:8464be95bf76 334
8mona 12:8464be95bf76 335
8mona 12:8464be95bf76 336 if (led_H>1.0)
8mona 12:8464be95bf76 337 {
8mona 12:8464be95bf76 338 led_H = led_H - 1.0;
8mona 12:8464be95bf76 339 }
8mona 12:8464be95bf76 340
8mona 12:8464be95bf76 341
8mona 12:8464be95bf76 342
8mona 12:8464be95bf76 343 float RGB[3];
8mona 12:8464be95bf76 344 float HSV[3];
8mona 12:8464be95bf76 345
8mona 12:8464be95bf76 346
8mona 12:8464be95bf76 347
8mona 12:8464be95bf76 348 HSV[0]=led_H;
8mona 12:8464be95bf76 349 HSV[1]=1;
8mona 12:8464be95bf76 350 HSV[2]=MAX_LED_BRI;
8mona 12:8464be95bf76 351
8mona 12:8464be95bf76 352
8mona 12:8464be95bf76 353 HSV2RGB(HSV,RGB);
8mona 12:8464be95bf76 354
8mona 12:8464be95bf76 355
8mona 12:8464be95bf76 356 LEDPWM[0]= 1.0-RGB[0]*1.1;
8mona 12:8464be95bf76 357 LEDPWM[1]= 1.0-RGB[1];
8mona 12:8464be95bf76 358 LEDPWM[2]= 1.0-RGB[2];
8mona 12:8464be95bf76 359
8mona 12:8464be95bf76 360
8mona 12:8464be95bf76 361
8mona 12:8464be95bf76 362 }
8mona 12:8464be95bf76 363
8mona 12:8464be95bf76 364 #endif
8mona 12:8464be95bf76 365
8mona 12:8464be95bf76 366
8mona 8:f7ad1d7176ba 367
8mona 5:2b9614aa1171 368
8mona 8:f7ad1d7176ba 369 void DisplayStatus()
8mona 8:f7ad1d7176ba 370 {
8mona 8:f7ad1d7176ba 371 lcd_.WriteValueXY("T%3d ",shaft_deg, 0,0);
8mona 8:f7ad1d7176ba 372 lcd_.WriteValue("V%2d",shaft_speed);
8mona 8:f7ad1d7176ba 373
8mona 8:f7ad1d7176ba 374 lcd_.WriteStringXY("F",9,0);
8mona 8:f7ad1d7176ba 375 for (int i=0;i<4;i++){
8mona 8:f7ad1d7176ba 376 lcd_.WriteValue("%d",sw_in[i]);
8mona 8:f7ad1d7176ba 377 }
8mona 8:f7ad1d7176ba 378 }
8mona 8:f7ad1d7176ba 379
8mona 8:f7ad1d7176ba 380 int MainIOloop()
8mona 8:f7ad1d7176ba 381 {
8mona 8:f7ad1d7176ba 382 static int cnt=0;
8mona 9:b58e7d72a91c 383 #ifdef ANGLE_ENABLE
8mona 12:8464be95bf76 384 //iqs62x.waitForIqsReady();
8mona 8:f7ad1d7176ba 385 iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers
8mona 12:8464be95bf76 386 //shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+ DEGREE_SHIFT;
8mona 12:8464be95bf76 387
8mona 12:8464be95bf76 388
8mona 12:8464be95bf76 389 shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+degree_shift;
8mona 9:b58e7d72a91c 390
8mona 12:8464be95bf76 391 if (shaft_deg<MIN_ANGLE)
8mona 12:8464be95bf76 392 {
8mona 12:8464be95bf76 393 shaft_deg=shaft_deg+360;
8mona 12:8464be95bf76 394 }
8mona 12:8464be95bf76 395 else if(shaft_deg>(MIN_ANGLE+360))
8mona 12:8464be95bf76 396 {
8mona 12:8464be95bf76 397 shaft_deg= shaft_deg-360;
8mona 12:8464be95bf76 398 }
8mona 12:8464be95bf76 399 /*
8mona 8:f7ad1d7176ba 400 if(shaft_deg<0)
8mona 8:f7ad1d7176ba 401 {
8mona 9:b58e7d72a91c 402 shaft_deg = shaft_deg+360; // offset 100deg to cancel error
8mona 8:f7ad1d7176ba 403 }
8mona 12:8464be95bf76 404 */
8mona 12:8464be95bf76 405 #endif
8mona 12:8464be95bf76 406
8mona 9:b58e7d72a91c 407 #ifdef ANGLE_ENABLE
8mona 8:f7ad1d7176ba 408 shaft_speed= ReadSpeed(iqs62x.registers);
8mona 8:f7ad1d7176ba 409 //lcd_.WriteValueXY("%3d ",k, 0,0);
8mona 9:b58e7d72a91c 410 #endif
8mona 8:f7ad1d7176ba 411
8mona 9:b58e7d72a91c 412 sw_in[0]= in_switchs[0];
8mona 9:b58e7d72a91c 413 sw_in[1]= !in_switchs[1];
8mona 9:b58e7d72a91c 414 sw_in[2]= in_switchs[2];
8mona 9:b58e7d72a91c 415 sw_in[3]= ! in_switchs[3];
8mona 9:b58e7d72a91c 416
8mona 9:b58e7d72a91c 417 /*
8mona 8:f7ad1d7176ba 418 for (int i=0;i<4;i=i++){
8mona 8:f7ad1d7176ba 419 sw_in[i]=!in_switchs[i];
8mona 9:b58e7d72a91c 420 sw_in[i+1]=!(in_switchs[i+1]);
8mona 8:f7ad1d7176ba 421 }
8mona 9:b58e7d72a91c 422 */
8mona 9:b58e7d72a91c 423
8mona 12:8464be95bf76 424
8mona 12:8464be95bf76 425 #ifdef LED_ENABLE
8mona 12:8464be95bf76 426 ShowLED(shaft_deg);
8mona 12:8464be95bf76 427 #endif
8mona 9:b58e7d72a91c 428
8mona 9:b58e7d72a91c 429
8mona 8:f7ad1d7176ba 430 cnt++;
8mona 9:b58e7d72a91c 431
8mona 9:b58e7d72a91c 432
8mona 9:b58e7d72a91c 433 bool statusL = motorL.status();
8mona 9:b58e7d72a91c 434 if (statusL & DRV8830_F_FAULT){
8mona 9:b58e7d72a91c 435 motorL.reset();
8mona 9:b58e7d72a91c 436 }
8mona 9:b58e7d72a91c 437
8mona 9:b58e7d72a91c 438 bool statusR = motorR.status();
8mona 9:b58e7d72a91c 439 if (statusR & DRV8830_F_FAULT){
8mona 9:b58e7d72a91c 440 motorR.reset();
8mona 9:b58e7d72a91c 441 }
8mona 9:b58e7d72a91c 442
8mona 9:b58e7d72a91c 443
8mona 9:b58e7d72a91c 444
8mona 8:f7ad1d7176ba 445 return cnt;
8mona 8:f7ad1d7176ba 446 }
8mona 8:f7ad1d7176ba 447
8mona 8:f7ad1d7176ba 448
8mona 8:f7ad1d7176ba 449 void TimerIsr()
8mona 8:f7ad1d7176ba 450 {
8mona 8:f7ad1d7176ba 451 //For LED Time-Sec display
8mona 8:f7ad1d7176ba 452 //wait_ms(5);
8mona 8:f7ad1d7176ba 453 g_timer++;
8mona 8:f7ad1d7176ba 454 //Displaylevel(val);
8mona 8:f7ad1d7176ba 455 }
8mona 8:f7ad1d7176ba 456
8mona 8:f7ad1d7176ba 457
8mona 8:f7ad1d7176ba 458 void flip() {
8mona 8:f7ad1d7176ba 459 static bool b = false;
8mona 8:f7ad1d7176ba 460
8mona 8:f7ad1d7176ba 461 if(b==false)
8mona 8:f7ad1d7176ba 462 {
8mona 8:f7ad1d7176ba 463 timer_.attach(&TimerIsr, TIMER_COUNT);
8mona 8:f7ad1d7176ba 464 }
8mona 8:f7ad1d7176ba 465
8mona 8:f7ad1d7176ba 466 else
8mona 8:f7ad1d7176ba 467 {
8mona 8:f7ad1d7176ba 468 timer_.detach();
8mona 8:f7ad1d7176ba 469 //Relay1=0;
8mona 8:f7ad1d7176ba 470 sp_index++;
8mona 8:f7ad1d7176ba 471 if (sp_index == 3)
8mona 8:f7ad1d7176ba 472 {
8mona 8:f7ad1d7176ba 473 sp_index = 0;
8mona 8:f7ad1d7176ba 474 }
8mona 8:f7ad1d7176ba 475 }
8mona 8:f7ad1d7176ba 476 b=!b;
8mona 8:f7ad1d7176ba 477
8mona 8:f7ad1d7176ba 478 }
8mona 5:2b9614aa1171 479
8mona 7:acb1074eaba6 480 int ReadDegree(char * buffer)
8mona 5:2b9614aa1171 481 {
8mona 7:acb1074eaba6 482 int ret=0;
8mona 7:acb1074eaba6 483 //(High bit + Low bit) * 360/65536
8mona 7:acb1074eaba6 484 //ret = ((buffer[0x80]<<8 + buffer[0x81])*0.00549316406 ;
8mona 7:acb1074eaba6 485 ret = (buffer[0x80]<<8 +buffer[0x81])/65536.0*360.0 ;
8mona 7:acb1074eaba6 486 return ret;
8mona 5:2b9614aa1171 487 }
8mona 5:2b9614aa1171 488
8mona 8:f7ad1d7176ba 489
8mona 7:acb1074eaba6 490 int ReadSpeed(char * buffer)
8mona 7:acb1074eaba6 491 {
8mona 7:acb1074eaba6 492 int ret=0;
8mona 7:acb1074eaba6 493 ret = (buffer[0x8E]);
8mona 7:acb1074eaba6 494 return ret;
8mona 7:acb1074eaba6 495 }
8mona 7:acb1074eaba6 496
8mona 7:acb1074eaba6 497 void Displaylevel (int deg)
8mona 7:acb1074eaba6 498 {
8mona 7:acb1074eaba6 499
8mona 7:acb1074eaba6 500 int level=deg>>5;
8mona 7:acb1074eaba6 501 lcd_.WriteStringXY("@",0,0);
8mona 7:acb1074eaba6 502 for (int i=0;i<12;i++)
8mona 7:acb1074eaba6 503 {
8mona 7:acb1074eaba6 504 if (i<level)
8mona 7:acb1074eaba6 505 {
8mona 7:acb1074eaba6 506 lcd_.WriteString("-");
8mona 7:acb1074eaba6 507 }
8mona 7:acb1074eaba6 508 else
8mona 7:acb1074eaba6 509 {
8mona 7:acb1074eaba6 510 lcd_.WriteString(" ");
8mona 7:acb1074eaba6 511 }
8mona 12:8464be95bf76 512 }
8mona 12:8464be95bf76 513
8mona 12:8464be95bf76 514 }
8mona 12:8464be95bf76 515
8mona 12:8464be95bf76 516 void HSV2RGB(const float *HSV, float *RGB) {
8mona 12:8464be95bf76 517 float h = HSV[0];
8mona 12:8464be95bf76 518 float s = HSV[1];
8mona 12:8464be95bf76 519 float v = HSV[2];
8mona 12:8464be95bf76 520
8mona 12:8464be95bf76 521 float r = v;
8mona 12:8464be95bf76 522 float g = v;
8mona 12:8464be95bf76 523 float b = v;
8mona 12:8464be95bf76 524
8mona 12:8464be95bf76 525 if (s > 0.0f) {
8mona 12:8464be95bf76 526 h *= 6.0f;
8mona 12:8464be95bf76 527 int i = (int) h;
8mona 12:8464be95bf76 528 float f = h - (float) i;
8mona 12:8464be95bf76 529 switch (i) {
8mona 12:8464be95bf76 530 default:
8mona 12:8464be95bf76 531 case 0:
8mona 12:8464be95bf76 532 g *= 1 - s * (1 - f);
8mona 12:8464be95bf76 533 b *= 1 - s;
8mona 12:8464be95bf76 534 break;
8mona 12:8464be95bf76 535 case 1:
8mona 12:8464be95bf76 536 r *= 1 - s * f;
8mona 12:8464be95bf76 537 b *= 1 - s;
8mona 12:8464be95bf76 538 break;
8mona 12:8464be95bf76 539 case 2:
8mona 12:8464be95bf76 540 r *= 1 - s;
8mona 12:8464be95bf76 541 b *= 1 - s * (1 - f);
8mona 12:8464be95bf76 542 break;
8mona 12:8464be95bf76 543 case 3:
8mona 12:8464be95bf76 544 r *= 1 - s;
8mona 12:8464be95bf76 545 g *= 1 - s * f;
8mona 12:8464be95bf76 546 break;
8mona 12:8464be95bf76 547 case 4:
8mona 12:8464be95bf76 548 r *= 1 - s * (1 - f);
8mona 12:8464be95bf76 549 g *= 1 - s;
8mona 12:8464be95bf76 550 break;
8mona 12:8464be95bf76 551 case 5:
8mona 12:8464be95bf76 552 g *= 1 - s;
8mona 12:8464be95bf76 553 b *= 1 - s * f;
8mona 12:8464be95bf76 554 break;
8mona 12:8464be95bf76 555 }
8mona 12:8464be95bf76 556 }
8mona 12:8464be95bf76 557
8mona 12:8464be95bf76 558 RGB[0] = r;
8mona 12:8464be95bf76 559 RGB[1] = g;
8mona 12:8464be95bf76 560 RGB[2] = b;
MikamiUitOpen 0:132eb47d57fb 561 }
8mona 12:8464be95bf76 562
8mona 12:8464be95bf76 563
8mona 12:8464be95bf76 564
MikamiUitOpen 0:132eb47d57fb 565