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@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?
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 | 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 |