Use IQS62X sensor and move motor by detected angle
Dependencies: DRV8830 IQS62x IQSDisplayTerminal UIT_ACM1602NI mbed
Fork of Nucleo_ACM1602_I2C_DC by
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?
User | Revision | Line number | New 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 |