重构部分代码
parent
59fc6c2bab
commit
6142e60a99
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,785 @@
|
||||||
|
MP2315S
|
||||||
|
|
||||||
|
3A, 24V, 500kHz, High-Efficiency,
|
||||||
|
Synchronous, Step-Down Converter
|
||||||
|
|
||||||
|
The Future of Analog IC Technology
|
||||||
|
|
||||||
|
DESCRIPTION FEATURES
|
||||||
|
|
||||||
|
The MP2315S is a high-efficiency, synchronous, • Wide 4.5V to 24V Operating Input Range
|
||||||
|
rectified, step-down, switch mode converter • 3A Load Current
|
||||||
|
with built-in, internal power MOSFETs. It is a • 110mΩ/55mΩ Low RDS(ON) Internal Power
|
||||||
|
next generation of the MP2315. It offers a very
|
||||||
|
compact solution to achieve 3A continuous MOSFETs
|
||||||
|
output current over a wide input supply range • Low Quiescent Current
|
||||||
|
with excellent load and line regulation. • High-Efficiency Synchronous Mode
|
||||||
|
|
||||||
|
The MP2315S uses synchronous mode Operation
|
||||||
|
operation for higher efficiency over the output • Fixed 500kHz Switching Frequency
|
||||||
|
current-load range. Current mode operation • AAM Power Save Mode
|
||||||
|
provides fast transient response and eases loop • Internal Soft Start
|
||||||
|
stabilization. • Output Over-Voltage Protection (OVP)
|
||||||
|
• Over-Current Protection (OCP) and Hiccup
|
||||||
|
Full protection features include over-current • Thermal Shutdown
|
||||||
|
protection (OCP), over-voltage protection • Output Adjustable from 0.8V
|
||||||
|
(OVP), and thermal shutdown. • Available in a TSOT23-8 Package
|
||||||
|
|
||||||
|
The MP2315S requires a minimal number of APPLICATIONS
|
||||||
|
readily available, standard, external
|
||||||
|
components and is available in a compact • Notebook Systems and I/O Power
|
||||||
|
TSOT23-8 package. • Digital Set-Top Boxes
|
||||||
|
• Flat Panel Television and Monitors
|
||||||
|
|
||||||
|
All MPS parts are lead-free, halogen-free, and adhere to the RoHS
|
||||||
|
directive. For MPS green status, please visit the MPS website under quality
|
||||||
|
assurance. “MPS” and “The Future of Analog IC Technology” are registered
|
||||||
|
trademarks of Monolithic Power Systems, Inc.
|
||||||
|
|
||||||
|
TYPICAL APPLICATION
|
||||||
|
|
||||||
|
100 0.10 1.00 10.00
|
||||||
|
95
|
||||||
|
90
|
||||||
|
85
|
||||||
|
80
|
||||||
|
75
|
||||||
|
70
|
||||||
|
65
|
||||||
|
60
|
||||||
|
0.01
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 1
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
ORDERING INFORMATION
|
||||||
|
|
||||||
|
Part Number* Package Top Marking
|
||||||
|
MP2315SGJ TSOT23-8 See below
|
||||||
|
|
||||||
|
* For Tape & Reel, add suffix –Z (e.g. MP2315SGJ–Z)
|
||||||
|
|
||||||
|
TOP MARKING
|
||||||
|
|
||||||
|
AQT: Product code
|
||||||
|
Y: Year code
|
||||||
|
|
||||||
|
PACKAGE REFERENCE
|
||||||
|
|
||||||
|
TOP VIEW
|
||||||
|
|
||||||
|
TSOT23-8
|
||||||
|
|
||||||
|
ABSOLUTE MAXIMUM RATINGS (1) Thermal Resistance (5) θJA θJC
|
||||||
|
|
||||||
|
VIN .................................................-0.3V to +26V TSOT23-8…………………....…..100…..55...°C/W
|
||||||
|
VSW..... -0.3V (-5V < 10ns) to +28V (30V < 10ns)
|
||||||
|
VBST ...................................................... VSW + 6V NOTES:
|
||||||
|
All other pins ........................... -0.3V to +5.5V (2) 1) Exceeding these ratings may damage the device.
|
||||||
|
Continuous power dissipation (TA = +25°C) (3) ... 2) For details on EN’s ABS MAX rating, please refer to the
|
||||||
|
................................................................. 1.25W
|
||||||
|
Junction temperature ................................150°C Enable Control section on page 9.
|
||||||
|
Lead temperature .....................................260°C 3) The maximum allowable power dissipation is a function of the
|
||||||
|
Storage temperature .................. -65°C to 150°C
|
||||||
|
maximum junction temperature TJ (MAX), the junction-to-
|
||||||
|
Recommended Operating Conditions (4) ambient thermal resistance θJA, and the ambient temperature
|
||||||
|
TA. The maximum allowable continuous power dissipation at
|
||||||
|
Supply voltage (VIN) ........................... 4.5 to 24V any ambient temperature is calculated by PD (MAX) = (TJ
|
||||||
|
Output voltage (VOUT) ..............0.8V to VIN * DMAX (MAX)-TA)/θJA. Exceeding the maximum allowable power
|
||||||
|
Operating junction temp (TJ). ... -40°C to +125°C dissipation produces an excessive die temperature, causing
|
||||||
|
the regulator to go into thermal shutdown. Internal thermal
|
||||||
|
shutdown circuitry protects the device from permanent
|
||||||
|
damage.
|
||||||
|
4) The device is not guaranteed to function outside of its
|
||||||
|
operating conditions.
|
||||||
|
5) Measured on JESD51-7, 4-layer PCB.
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 2
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
ELECTRICAL CHARACTERISTICS
|
||||||
|
|
||||||
|
VIN = 12V, TJ = -40°C to +125°C.(6) Typical value is tested at TJ = +25°C, unless otherwise noted.
|
||||||
|
|
||||||
|
Parameter Symbol Condition Min Typ Max Units
|
||||||
|
|
||||||
|
Supply current (shutdown) IIN VEN = 0V, TJ = 25°C 1 μA
|
||||||
|
Supply current (quiescent)
|
||||||
|
Iq VEN = 2V, VFB = 0.85V, 120 μA
|
||||||
|
AAM = 0.4V
|
||||||
|
|
||||||
|
HS switch on resistance HSRDS-ON VBST-SW = 5V 110 mΩ
|
||||||
|
LS switch on resistance LSRDS-ON VCC = 5V
|
||||||
|
VEN = 0V, VSW = 12V, 55 mΩ
|
||||||
|
Switch leakage SWLKG TJ = 25°C
|
||||||
|
Duty cycle = 40% 1 μA
|
||||||
|
Current limit ILIMIT
|
||||||
|
Oscillator frequency fSW VFB = 750mV 4.5 5.5 6.5 A
|
||||||
|
Foldback frequency fFB VFB = 200mV
|
||||||
|
Maximum duty cycle DMAX VFB = 750mV 400 500 600 kHz
|
||||||
|
Minimum on time (7) TON_MIN
|
||||||
|
0.5 fSW
|
||||||
|
|
||||||
|
90 95 %
|
||||||
|
|
||||||
|
60 ns
|
||||||
|
|
||||||
|
Feedback voltage VFB 783 791 800 mV
|
||||||
|
|
||||||
|
Feedback current IFB VFB = 820mV 10 50 nA
|
||||||
|
EN rising threshold VEN_RISING
|
||||||
|
EN hysteresis VEN_HYS 1.26 1.4 1.54 V
|
||||||
|
|
||||||
|
150 mV
|
||||||
|
|
||||||
|
EN input current IEN VEN = 2V 1 2 3 μA
|
||||||
|
VEN = 0
|
||||||
|
EN turn-off delay ENTd-off 0 50 nA
|
||||||
|
VIN under-voltage lockout INUVVth
|
||||||
|
threshold rising 5 9 13 μs
|
||||||
|
|
||||||
|
3.85 4.05 4.25 V
|
||||||
|
|
||||||
|
VIN under-voltage lockout INUVHYS 600 750 900 mV
|
||||||
|
threshold hysteresis 4.85 5.35
|
||||||
|
0.8 5.1 2.2 V
|
||||||
|
VCC regulator VCC ICC = 5mA 1.5 %
|
||||||
|
VCC load regulation 10% to 90% 115% 1.5 125% ms
|
||||||
|
Soft-start period TSS 104% 150 114% ºC
|
||||||
|
Thermal shutdown (7) TSD FB voltage 20 ºC
|
||||||
|
Thermal hysteresis (7) THYS FB voltage 6.7 μA
|
||||||
|
AAM source current IAAM 120% VREF
|
||||||
|
OVP rising threshold OVH_RISE 109% VREF
|
||||||
|
OVP falling threshold OVL_FALL 2 μs
|
||||||
|
OVP delay (7) OVDEY
|
||||||
|
|
||||||
|
NOTES:
|
||||||
|
6) Not tested in production. Guaranteed by over-temperature correlation.
|
||||||
|
7) Guarantee by engineering sample characterization.
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 3
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
TYPICAL CHARACTERISTICS
|
||||||
|
|
||||||
|
VIN = 19V, VOUT = 5V, L = 4.9μH, TA = 25°C, unless otherwise noted.
|
||||||
|
|
||||||
|
100 100 100
|
||||||
|
|
||||||
|
95 95 95
|
||||||
|
|
||||||
|
90 90
|
||||||
|
90
|
||||||
|
|
||||||
|
85
|
||||||
|
|
||||||
|
85 85 80
|
||||||
|
|
||||||
|
80 80 75
|
||||||
|
|
||||||
|
75 75 70
|
||||||
|
|
||||||
|
70 70 65
|
||||||
|
|
||||||
|
60
|
||||||
|
|
||||||
|
65 65 55
|
||||||
|
|
||||||
|
60 60 50
|
||||||
|
0.01
|
||||||
|
0.01 0.10 1.00 10.00 0.01 0.10 1.00 10.00 0.10 1.00 10.00
|
||||||
|
|
||||||
|
0.9 6
|
||||||
|
|
||||||
|
0.9
|
||||||
|
|
||||||
|
0.7 0.7 5.8
|
||||||
|
|
||||||
|
0.5 0.5
|
||||||
|
|
||||||
|
5.6
|
||||||
|
|
||||||
|
0.3 0.3
|
||||||
|
|
||||||
|
0.1 0.1 5.4
|
||||||
|
|
||||||
|
-0.1 -0.1
|
||||||
|
|
||||||
|
5.2
|
||||||
|
|
||||||
|
-0.3 -0.3
|
||||||
|
|
||||||
|
-0.5 -0.5 5
|
||||||
|
|
||||||
|
0 1 2 3 6 8 10 12 14 16 18 20 22 24 0 10 20 30 40 50 60 70 80 90 100
|
||||||
|
|
||||||
|
150 200 60
|
||||||
|
150 50
|
||||||
|
140 100
|
||||||
|
|
||||||
|
50 40
|
||||||
|
0
|
||||||
|
130 4 8 12 16 20 24
|
||||||
|
|
||||||
|
30
|
||||||
|
|
||||||
|
120
|
||||||
|
|
||||||
|
20
|
||||||
|
|
||||||
|
110 10
|
||||||
|
|
||||||
|
100 0
|
||||||
|
4
|
||||||
|
8 12 16 20 24 1 1.5 2 2.5 3
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 4
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
TYPICAL PERFORMANCE CHARACTERISTICS
|
||||||
|
|
||||||
|
VIN = 19V, VOUT = 5V, L = 4.9μH, TA = 25°C, unless otherwise noted.
|
||||||
|
|
||||||
|
VOUT VOUT VOUT
|
||||||
|
2V/div. 2V/div. 2V/div.
|
||||||
|
|
||||||
|
VIN VIN VIN
|
||||||
|
10V/div. 5V/div. 10V/div.
|
||||||
|
|
||||||
|
VSW VSW VSW
|
||||||
|
10V/div. 10V/div. 10V/div.
|
||||||
|
IINDUCTOR IINDUCTOR IINDUCTOR
|
||||||
|
|
||||||
|
2A/div. 2A/div. 5A/div.
|
||||||
|
|
||||||
|
VOUT VOUT VOUT
|
||||||
|
2V/div. 2V/div. 2V/div.
|
||||||
|
|
||||||
|
VIN VEN VEN
|
||||||
|
5V/div. 5V/div. 5V/div.
|
||||||
|
|
||||||
|
VSW VSW VSW
|
||||||
|
5V/div. 20V/div. 20V/div.
|
||||||
|
IINDUCTOR IINDUCTOR IINDUCTOR
|
||||||
|
5A/div.
|
||||||
|
2A/div. 2A/div.
|
||||||
|
|
||||||
|
VOUT VOUT VOUT/AC
|
||||||
|
2V/div. 2V/div. 50mV/div.
|
||||||
|
|
||||||
|
VEN VEN VIN/AC
|
||||||
|
5V/div. 5V/div. 100mV/div.
|
||||||
|
|
||||||
|
VSW VSW VSW
|
||||||
|
20V/div. 20V/div. 20V/div.
|
||||||
|
IINDUCTOR IINDUCTOR IINDUCTOR
|
||||||
|
|
||||||
|
5A/div. 5A/div. 2A/div.
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 5
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
TYPICAL PERFORMANCE CHARACTERISTICS (continued)
|
||||||
|
|
||||||
|
VIN = 19V, VOUT = 5V, L = 4.9μH, TA = 25°C, unless otherwise noted.
|
||||||
|
|
||||||
|
VOUT/AC VOUT VOUT
|
||||||
|
20mV/div. 200mV/div. 2V/div.
|
||||||
|
|
||||||
|
VIN/AC IOUT VSW
|
||||||
|
200mV/div. 2A/div. 20V/div.
|
||||||
|
|
||||||
|
VSW IINDUCTOR
|
||||||
|
20V/div. 5A/div.
|
||||||
|
|
||||||
|
IINDUCTOR
|
||||||
|
2A/div.
|
||||||
|
|
||||||
|
60 180
|
||||||
|
40
|
||||||
|
20 120
|
||||||
|
0
|
||||||
|
VOUT -20 60
|
||||||
|
2V/div. -40
|
||||||
|
-60 0
|
||||||
|
VSW 1000
|
||||||
|
20V/div. -60
|
||||||
|
|
||||||
|
IINDUCTOR -120
|
||||||
|
5A/div.
|
||||||
|
|
||||||
|
10000 -180
|
||||||
|
100000 1000000
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 6
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
PIN FUNCTIONS
|
||||||
|
|
||||||
|
Pin # Name Description
|
||||||
|
1 AAM
|
||||||
|
Advanced asynchronous modulation. Connect AAM to a voltage supply through a
|
||||||
|
2 IN resistor divider to force the MP2315S into non-synchronous mode under light-load
|
||||||
|
3 SW conditions. Tie AAM to VCC or leave AAM floating to disable AAM mode and force the
|
||||||
|
4 GND MP2315S into CCM.
|
||||||
|
5 BST
|
||||||
|
6 EN Supply voltage. The MP2315S operates with a 4.5V to 24V input rail. C1 is needed to
|
||||||
|
7 VCC decouple the input rail. Connect using a wide PCB trace.
|
||||||
|
|
||||||
|
8 FB Switch output. Connect using a wide PCB trace.
|
||||||
|
|
||||||
|
System ground. GND is the reference ground of the regulated output voltage. GND
|
||||||
|
requires special consideration during PCB layout. Connect GND with copper traces and
|
||||||
|
vias.
|
||||||
|
|
||||||
|
Bootstrap. A capacitor and a resistor are required between SW and BST to form a
|
||||||
|
floating supply across the high-side switch driver.
|
||||||
|
|
||||||
|
Enable. Drive EN high to enable the MP2315S.
|
||||||
|
|
||||||
|
Internal bias supply, internal 5.1V LDO output. Decouple VCC with a 0.1μF - 0.22μF
|
||||||
|
capacitor. The capacitance should be no more than 0.22μF.
|
||||||
|
|
||||||
|
Feedback. Connect FB to the tap of an external resistor divider from the output to GND
|
||||||
|
to set the output voltage. The frequency foldback comparator lowers the oscillator
|
||||||
|
frequency when the FB voltage is below 400mV to prevent current-limit runaway during a
|
||||||
|
short-circuit fault condition.
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 7
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
BLOCK DIAGRAM
|
||||||
|
|
||||||
|
Figure 1: Functional Block Diagram
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 8
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
OPERATION Under light-load conditions, the value of VCOMP
|
||||||
|
is low. When VCOMP is less than VAAM, and VFB is
|
||||||
|
The MP2315S is a high-efficiency, synchronous, less than VREF, VCOMP ramps up until it exceeds
|
||||||
|
rectified, step-down, switch mode converter VAAM. During this time, the internal clock is
|
||||||
|
with built-in internal power MOSFETs. It offers a blocked, and the MP2315S skips some pulses
|
||||||
|
very compact solution that achieves 3A of for pulse frequency modulation (PFM) mode
|
||||||
|
continuous output current with excellent load and achieves a light-load power save.
|
||||||
|
and line regulation over a wide input supply
|
||||||
|
range. Figure 2: Simplified AAM Control Logic
|
||||||
|
|
||||||
|
When the MP2315S operates in a fixed Enable Control (EN)
|
||||||
|
frequency, the peak-current control mode Enable (EN) is a digital control that turns the
|
||||||
|
regulates the output voltage. A pulse width regulator on and off. Drive EN high to turn on
|
||||||
|
modulation (PWM) cycle is initiated by the the regulator; drive EN low to turn off the
|
||||||
|
internal clock. The integrated high-side power regulator. An internal 1MΩ resistor from EN to
|
||||||
|
MOSFET (HS-FET) turns on and remains on GND allows EN to be floated to shut down the
|
||||||
|
until its current reaches the value set by the chip.
|
||||||
|
COMP voltage (VCOMP). When the power switch
|
||||||
|
is off, it remains off until the next clock cycle EN is clamped internally using a 5.6V series
|
||||||
|
begins. If the current in the power MOSFET Zener diode. Connect the EN input through a
|
||||||
|
does not reach the COMP set current value pull-up resistor to the voltage on VIN to limit the
|
||||||
|
within 95% of one PWM period, the power EN input current below 100µA. For example,
|
||||||
|
MOSFET is forced off. with 19V connected to VIN, RPULLUP ≥ (19V -
|
||||||
|
5.6V) ÷ 100µA = 134kΩ.
|
||||||
|
Internal Regulator
|
||||||
|
Connecting EN directly to a voltage source
|
||||||
|
Most of the internal circuitries are powered by without a pull-up resistor requires limiting the
|
||||||
|
the 5.1V internal regulator. This regulator takes amplitude of the voltage source below 5.5V to
|
||||||
|
the VIN input and operates in the full VIN range. prevent damage to the Zener diode.
|
||||||
|
When VIN is greater than 5.1V, the output of the
|
||||||
|
regulator is in full regulation. When VIN drops Under-Voltage Lockout (UVLO)
|
||||||
|
below 5.1V, the output decreases. A 0.1µF Under-voltage lockout (UVLO) is implemented
|
||||||
|
ceramic capacitor is required for decoupling. to prevent the chip from operating at an
|
||||||
|
insufficient supply voltage. The MP2315S
|
||||||
|
Error Amplifier (EA) UVLO comparator monitors the output voltage
|
||||||
|
of the internal regulator (VCC). The UVLO
|
||||||
|
The error amplifier compares the FB voltage rising threshold is about 4.05V, while its falling
|
||||||
|
with the internal 0.791V reference (REF) and threshold is 3.3V.
|
||||||
|
outputs a COMP voltage which is used to
|
||||||
|
control the power MOSFET current. The Internal Soft-Start (SS)
|
||||||
|
optimized internal compensation network The soft start (SS) is implemented to prevent
|
||||||
|
minimizes the external component count and the converter output voltage from overshooting
|
||||||
|
simplifies the control loop design. during start-up. When the chip starts up, the
|
||||||
|
internal circuitry generates a soft-start voltage
|
||||||
|
AAM Operation that ramps up from 0V. The soft-start period
|
||||||
|
lasts until the voltage on the soft-start capacitor
|
||||||
|
The MP2315S uses advanced asynchronous exceeds the reference voltage of 0.791V. At
|
||||||
|
modulation (AAM) power save mode in light this point, the reference voltage takes over. The
|
||||||
|
load. Set the AAM voltage with the tap of an soft-start time is internally set to around 1.5ms.
|
||||||
|
external resistor divider from VCC to GND.
|
||||||
|
Under heavy-load conditions, VCOMP is higher
|
||||||
|
than VAAM. When the clock goes low, the HS-
|
||||||
|
FET turns on and remains on until VILsense
|
||||||
|
reaches the value set by VCOMP. The internal
|
||||||
|
clock resets whenever VCOMP is higher than
|
||||||
|
VAAM.
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 9
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
Output Over-Voltage Protection (OVP) Floating Driver and Bootstrap Charging
|
||||||
|
The floating power MOSFET driver is powered
|
||||||
|
The MP2315S monitors the FB voltage to by an external bootstrap capacitor. This floating
|
||||||
|
detect output over-voltage. When the FB driver has its own UVLO protection with a rising
|
||||||
|
voltage rises higher than 120% of the reference threshold of 2.2V and a hysteresis of 150mV.
|
||||||
|
voltage, the MP2315S enters a dynamic The bootstrap capacitor voltage is regulated
|
||||||
|
regulation period. During this period, the IC internally by VIN through D1, R3, C3, L1, and C2
|
||||||
|
forces the low-side MOSFET (LS-FET) on until (see Figure 3). If VIN - VSW is more than 5V, U2
|
||||||
|
a -800mA negative current limit is achieved. regulates M3 to maintain a 5V BST voltage
|
||||||
|
This discharges the output to keep it within the across C3.
|
||||||
|
normal range. The MP2315S exits dynamic
|
||||||
|
regulation when FB falls below 109% of the R3
|
||||||
|
reference voltage. 3
|
||||||
|
|
||||||
|
Over-Current Protection (OCP) and Hiccup Figure 3: Internal Bootstrap Charging Circuit
|
||||||
|
Start-Up and Shutdown
|
||||||
|
The MP2315S uses a cycle-by-cycle over- If both VIN and EN are higher than their
|
||||||
|
current limit when the inductor current peak appropriate thresholds, the chip starts up. The
|
||||||
|
value exceeds the set current-limit threshold. reference block starts first, generating a stable
|
||||||
|
The output voltage begins dropping until FB is reference voltage and current. The internal
|
||||||
|
below the under-voltage (UV) threshold, regulator is then enabled. The regulator
|
||||||
|
typically 50% below the reference. Once UV is provides a stable supply for the remaining
|
||||||
|
triggered, the MP2315S enters hiccup mode to circuitries.
|
||||||
|
restart the part periodically. This protection Three events can shut down the chip: EN low,
|
||||||
|
mode is especially useful when the output is VIN low, and thermal shutdown. In the shutdown
|
||||||
|
dead-shorted to ground. The average short- procedure, the signaling path is blocked first to
|
||||||
|
circuit current is reduced greatly to alleviate the prevent any fault triggering. VCOMP and the
|
||||||
|
thermal issue and protect the regulator. The internal supply rail are then pulled down. The
|
||||||
|
MP2315S exits hiccup mode once the over- floating driver is not subject to this shutdown
|
||||||
|
current condition is removed. command.
|
||||||
|
|
||||||
|
Pre-Bias Start-Up
|
||||||
|
|
||||||
|
The MP2315S is designed for monotonic start-
|
||||||
|
up into pre-biased loads. If the output is pre-
|
||||||
|
biased to a certain voltage during start-up, the
|
||||||
|
BST voltage is refreshed and charged, and the
|
||||||
|
voltage on the soft-start capacitor is charged as
|
||||||
|
well. If the BST voltage exceeds its rising
|
||||||
|
threshold voltage, and the soft-start capacitor
|
||||||
|
voltage exceeds the sensed output voltage at
|
||||||
|
FB, the part begins working normally.
|
||||||
|
|
||||||
|
Thermal Shutdown
|
||||||
|
|
||||||
|
Thermal shutdown is implemented to prevent
|
||||||
|
the chip from operating at exceedingly high
|
||||||
|
temperatures. When the silicon die temperature
|
||||||
|
is higher than 150°C, the entire chip shuts down.
|
||||||
|
When the temperature is lower than its lower
|
||||||
|
threshold, typically 130°C, the chip is enabled
|
||||||
|
again.
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 10
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
APPLICATION INFORMATION
|
||||||
|
|
||||||
|
Setting the Output Voltage Choose the inductor current to be
|
||||||
|
approximately 40% of the maximum load
|
||||||
|
An external resistor divider is used to set the current. The maximum inductor peak current
|
||||||
|
output voltage. The feedback resistor (R1) also can be calculated with Equation (3):
|
||||||
|
sets the feedback loop bandwidth with the
|
||||||
|
internal compensation capacitor (see Typical IL(MAX) = ILOAD + ΔIL (3)
|
||||||
|
Application on page 1). R2 can then be 2
|
||||||
|
calculated with Equation (1):
|
||||||
|
Under light-load conditions below 100mA, a
|
||||||
|
R2 = R1 larger inductance is recommended for better
|
||||||
|
VOUT − 1 efficiency.
|
||||||
|
|
||||||
|
0.791V (1) Setting the AAM Voltage
|
||||||
|
|
||||||
|
The T-type network is highly recommended The AAM voltage is used to set the transition
|
||||||
|
(see Figure 4). point from AAM to PWM. It should be chosen to
|
||||||
|
provide the best combination of efficiency,
|
||||||
|
8 RT R1 stability, ripple, and transient.
|
||||||
|
|
||||||
|
FB VOUT
|
||||||
|
|
||||||
|
R2 If the AAM voltage is set low, then the stability
|
||||||
|
and ripple improve, but AAM mode and
|
||||||
|
Figure 4: T-Type Network transient efficiency degrade. Likewise, if the
|
||||||
|
AAM voltage is set high, then AAM mode and
|
||||||
|
Table 1 lists the recommended T-type resistor transient efficiency improves, but the stability
|
||||||
|
values for common output voltages. and ripple degrade.
|
||||||
|
|
||||||
|
Table 1: Resistor Selection for Common Output Adjust the AAM threshold by connecting divider
|
||||||
|
Voltages(8) resistors from VCC to GND. Note that there is a
|
||||||
|
6.7µA current source at AAM (see Figure 5).
|
||||||
|
|
||||||
|
VOUT (V) R1 (kΩ) R2 (kΩ) Rt (kΩ)
|
||||||
|
1.05 20.5 62 100
|
||||||
|
1.2 20.5 39.2 75
|
||||||
|
1.8 40.2 31.6 59
|
||||||
|
2.5 40.2 18.7 40.2
|
||||||
|
3.3 40.2 12.7 33
|
||||||
|
5 40.2 7.5 20
|
||||||
|
|
||||||
|
NOTE: Figure 5: AAM Network
|
||||||
|
8) The recommended parameters are based on a 44µF output
|
||||||
|
Generally, VAAM can be calculated with Equation
|
||||||
|
capacitor. A different input voltage, output inductor value, and (4):
|
||||||
|
output capacitor value may affect the selection of R1, R2, and
|
||||||
|
Rt. For additional component parameters, please refer to the
|
||||||
|
Typical Application Circuits section on pages 15 and 16.
|
||||||
|
|
||||||
|
Selecting the Inductor VAAM = R5 × (VCC + 6.7μA ×R4 )
|
||||||
|
R4 + R5
|
||||||
|
A 1µH to 10µH inductor with a DC current rating (4)
|
||||||
|
at least 25% percent higher than the maximum
|
||||||
|
load current is recommended for most R5 should be no larger than 20k.
|
||||||
|
applications. For the highest efficiency, the
|
||||||
|
inductor DC resistance should be less than
|
||||||
|
20mΩ. For most designs, the inductance value
|
||||||
|
can be derived using Equation (2):
|
||||||
|
|
||||||
|
L1 = VOUT × (VIN − VOUT )
|
||||||
|
VIN × ΔIL × fOSC
|
||||||
|
(2)
|
||||||
|
|
||||||
|
Where ΔIL is the inductor ripple current.
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 11
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
The optimized AAM can be found in Figure 6. The input voltage ripple caused by the
|
||||||
|
capacitance can be estimated with Equation (7):
|
||||||
|
0. 7
|
||||||
|
|
||||||
|
0. 6 ΔVIN = ILOAD × VOUT × ⎛⎜1− VOUT ⎞
|
||||||
|
fS × C1 VIN ⎝ VIN ⎟
|
||||||
|
0. 5 ⎠ (7)
|
||||||
|
|
||||||
|
0. 4 Selecting the Output Capacitor
|
||||||
|
|
||||||
|
0. 3 The output capacitor (C2) is required to
|
||||||
|
maintain the DC output voltage. Ceramic,
|
||||||
|
0. 2 tantalum, or low ESR electrolytic capacitors are
|
||||||
|
recommended. Low ESR capacitors are
|
||||||
|
0. 1 recommended to keep the output voltage ripple
|
||||||
|
low. The output voltage ripple can be estimated
|
||||||
|
0 with Equation (8):
|
||||||
|
|
||||||
|
0 2 4 6 8
|
||||||
|
|
||||||
|
Figure 6: AAM Selection for Common Output ΔVOUT = VOUT × ⎛ VOUT ⎞⎛ + 8 × 1 C2 ⎞
|
||||||
|
Voltages (VIN = 4.5V - 24V) fS × L1 ⎜1− VIN ⎟ × ⎜RESR fS × ⎟
|
||||||
|
⎝ ⎠⎝ ⎠ (8)
|
||||||
|
|
||||||
|
Selecting the Input Capacitor Where L1 is the inductor value and RESR is the
|
||||||
|
equivalent series resistance (ESR) value of the
|
||||||
|
The input current to the step-down converter is output capacitor.
|
||||||
|
discontinuous and therefore requires a
|
||||||
|
capacitor to supply AC current to the step-down For ceramic capacitors, the impedance at the
|
||||||
|
converter while maintaining the DC input switching frequency is dominated by the
|
||||||
|
voltage. Use low ESR capacitors for best capacitance. The output voltage ripple is mainly
|
||||||
|
performance. Ceramic capacitors with X5R or caused by the capacitance. For simplification,
|
||||||
|
X7R dielectrics are highly recommended the output voltage ripple can be estimated with
|
||||||
|
because of their low ESR and small Equation (9):
|
||||||
|
temperature coefficients. For most applications,
|
||||||
|
a 22µF capacitor is sufficient. ΔVOUT = VOUT × ⎛ − VOUT ⎞
|
||||||
|
fS2 × L1 ⎜1 VIN ⎟
|
||||||
|
Since the input capacitor (C1) absorbs the input 8 × × C2 ⎝ ⎠ (9)
|
||||||
|
switching current, it requires an adequate ripple
|
||||||
|
current rating. The RMS current in the input In the case of tantalum or electrolytic capacitors,
|
||||||
|
capacitor can be estimated with Equation (5): the ESR dominates the impedance at the
|
||||||
|
switching frequency. For simplification, the
|
||||||
|
IC1 = ILOAD × VOUT ×⎜⎜⎛1− VOUT ⎟⎞ output ripple can be approximated with
|
||||||
|
VIN VIN ⎟ Equation (10):
|
||||||
|
⎝ ⎠ (5)
|
||||||
|
|
||||||
|
The worst-case condition occurs at VIN = ΔVOUT = VOUT × ⎛ 1− VOUT ⎞ × RESR
|
||||||
|
2VOUT, shown in Equation (6): fS × L1 ⎝⎜ VIN ⎟⎠
|
||||||
|
(10)
|
||||||
|
|
||||||
|
IC1 = ILOAD (6) The characteristics of the output capacitor also
|
||||||
|
2 affect the stability of the regulation system. The
|
||||||
|
MP2315S can be optimized for a wide range of
|
||||||
|
For simplification, choose an input capacitor capacitance and ESR values.
|
||||||
|
with an RMS current rating greater than half of
|
||||||
|
the maximum load current. External Bootstrap Diode
|
||||||
|
|
||||||
|
The input capacitor can be electrolytic, tantalum, An external bootstrap diode may enhance the
|
||||||
|
or ceramic. When using electrolytic or tantalum efficiency of the regulator. The applicable
|
||||||
|
capacitors, a small, high-quality ceramic conditions of the external BST diode are:
|
||||||
|
capacitor (i.e.: 0.1μF) should be placed as close
|
||||||
|
to the IC as possible. When using ceramic • VOUT is 5V or 3.3V
|
||||||
|
capacitors, ensure that they have enough • Duty cycle is high: D = VOUT > 65%
|
||||||
|
capacitance to a provide sufficient charge to
|
||||||
|
prevent excessive voltage ripple at input. VIN
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 12
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
In these cases, an external BST diode is
|
||||||
|
recommended from VCC to BST (see Figure 7).
|
||||||
|
|
||||||
|
Figure 7: Add Optional External Bootstrap Diode Figure 8: Sample Board Layout
|
||||||
|
to Enhance Efficiency
|
||||||
|
Design Example
|
||||||
|
The recommended external BST diode is
|
||||||
|
1N4148, and the recommended BST capacitor Table 2 is a design example following the
|
||||||
|
is 0.1 - 1μF. application guidelines for the specifications
|
||||||
|
below:
|
||||||
|
PCB Layout Guidelines (9)
|
||||||
|
Table 2: Design Example
|
||||||
|
Efficient PCB layout is critical for stable
|
||||||
|
operation. For best results, refer to Figure 8 and VIN 19V
|
||||||
|
follow the guidelines below: VOUT 5V
|
||||||
|
3A
|
||||||
|
1. Keep the connection of the input ground IO
|
||||||
|
and GND as short and wide as possible.
|
||||||
|
|
||||||
|
2. Keep the connection of the input capacitor
|
||||||
|
and IN as short and wide as possible.
|
||||||
|
|
||||||
|
3. Place the VCC capacitor as close to VCC
|
||||||
|
and GND as possible.
|
||||||
|
|
||||||
|
4. Make the trace length of VCC - VCC
|
||||||
|
capacitor anode - VCC capacitor cathode -
|
||||||
|
IC GND as short as possible.
|
||||||
|
|
||||||
|
5. Ensure that all feedback connections are
|
||||||
|
short and direct.
|
||||||
|
|
||||||
|
6. Place the feedback resistors and
|
||||||
|
compensation components as close to the
|
||||||
|
IC as possible.
|
||||||
|
|
||||||
|
7. Route SW away from sensitive analog
|
||||||
|
areas, such as FB.
|
||||||
|
|
||||||
|
NOTE:
|
||||||
|
9) The recommended layout is based on Figure 9 on page 15.
|
||||||
|
|
||||||
|
The detailed application schematic is shown in
|
||||||
|
Figure 9. The typical performance and circuit
|
||||||
|
waveforms are shown in the Typical
|
||||||
|
Performance Characteristics section. For more
|
||||||
|
device applications, please refer to the related
|
||||||
|
evaluation board datasheets.
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 13
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
TYPICAL APPLICATION CIRCUITS
|
||||||
|
|
||||||
|
Figure 9: VIN = 6.5V - 24V, VOUT = 5V, IOUT = 3A
|
||||||
|
|
||||||
|
Figure 10: VIN = 4.5V - 24V, VOUT = 3.3V, IOUT = 3A
|
||||||
|
|
||||||
|
Figure 11: VIN = 4.5V - 24V, VOUT = 2.5V, IOUT = 3A
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 14
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
Figure 12: VIN = 4.5V - 24V, VOUT = 1.8V, IOUT = 3A
|
||||||
|
|
||||||
|
Figure 13: VIN = 4.5V - 24V, VOUT = 1.2V, IOUT = 3A
|
||||||
|
|
||||||
|
Figure 14: VIN = 4.5V - 24V, VOUT = 1.05V, IOUT = 3A
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 15
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
MP2315S – 3A, 24V, SYNCHRONOUS, STEP-DOWN CONVERTER
|
||||||
|
|
||||||
|
PACKAGE INFORMATION
|
||||||
|
|
||||||
|
TSOT23-8
|
||||||
|
|
||||||
|
See note 7 IAAAA
|
||||||
|
EXAMPLE
|
||||||
|
TOP MARK
|
||||||
|
|
||||||
|
PIN 1 ID
|
||||||
|
|
||||||
|
TOP VIEW RECOMMENDED LAND PATTERN
|
||||||
|
|
||||||
|
SEATING PLANE SEE DETAIL''A''
|
||||||
|
|
||||||
|
FRONT VIEW SIDE VIEW
|
||||||
|
|
||||||
|
DETAIL ''A'' NOTE:
|
||||||
|
|
||||||
|
1) ALL DIMENSIONS ARE IN MILLIMETERS.
|
||||||
|
2) PACKAGE LENGTH DOES NOT INCLUDE MOLD
|
||||||
|
FLASH, PROTRUSION OR GATE BURR.
|
||||||
|
3) PACKAGE WIDTH DOES NOT INCLUDE
|
||||||
|
INTERLEAD FLASH OR PROTRUSION.
|
||||||
|
4) LEAD COPLANARITY(BOTTOM OF LEADS
|
||||||
|
AFTER FORMING) SHALL BE 0.10 MILLIMETERS
|
||||||
|
MAX.
|
||||||
|
5) JEDEC REFERENCE IS MO-193, VARIATION BA.
|
||||||
|
6) DRAWING IS NOT TO SCALE.
|
||||||
|
7) PIN 1 IS LOWER LEFT PIN WHEN READING TOP
|
||||||
|
MARK FROM LEFT TO RIGHT, (SEE EXAMPLE TOP
|
||||||
|
MARK)
|
||||||
|
|
||||||
|
NOTICE: The information in this document is subject to change without notice. Users should warrant and guarantee that third
|
||||||
|
party Intellectual Property rights are not infringed upon when integrating MPS products into any application. MPS will not
|
||||||
|
assume any legal responsibility for any said applications.
|
||||||
|
|
||||||
|
MP2315S Rev. 1.0 www.MonolithicPower.com 16
|
||||||
|
|
||||||
|
12/21/2015 MPS Proprietary Information. Patent Protected. Unauthorized Photocopy and Duplication Prohibited.
|
||||||
|
|
||||||
|
© 2015 MPS. All Rights Reserved.
|
||||||
|
|
||||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,7 @@
|
||||||
|
## 1、Pibot电源稳压问题
|
||||||
|
|
||||||
|
`Pibot`采用的是`LM2596S`的稳压方案，1、输入电压范围：直流`3.2V `至 `40V` (输入的电压必须比要输出的电压高`1.5V`以上。不能升压)产品特点。输出电压范围：直流 `1.25V`至 `35V` 电压连续可调，高效率(可高达92%)输出电流可达`3A`。长时间工作建议在`2.5A`以内的电流使用，同时加上散热片(`10W`以上输出)；由于是降压模块,为了保证输出稳定,请保持最小`1.5V`压差。
|
||||||
|
|
||||||
|
**暂时不明白的问题在于，为什么输入`24V`电压多次导致`STM32`损坏。**
|
||||||
|
|
||||||
|
|
||||||
File diff suppressed because one or more lines are too long
|
|
@ -33,19 +33,19 @@ void SysTick_Handler(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
float PB_Get_System_Time(void)
|
float getSystemTime(void)
|
||||||
{
|
{
|
||||||
float count , time;
|
float count , time;
|
||||||
count = (float)( (reload + 1 - SysTick->VAL) + (float)(reload + 1) * count_time );
|
count = (float)( (reload + 1 - SysTick->VAL) + (float)(reload + 1) * count_time);
|
||||||
time = count/count_us;
|
time = count/count_us;
|
||||||
return time;
|
return time;
|
||||||
}
|
}
|
||||||
|
|
||||||
float PB_Get_Dtime(void)
|
float getDeltaTime(void)
|
||||||
{
|
{
|
||||||
static float lasttime ;
|
static float lasttime;
|
||||||
float temp1,temp2;
|
float temp1,temp2;
|
||||||
temp1 = PB_Get_System_Time();
|
temp1 = getSystemTime();
|
||||||
temp2 = temp1 - lasttime;
|
temp2 = temp1 - lasttime;
|
||||||
if(temp2 < 0)
|
if(temp2 < 0)
|
||||||
{
|
{
|
||||||
|
|
|
||||||
|
|
@ -8,12 +8,12 @@ extern "C" {
|
||||||
#include "stm32f10x.h"
|
#include "stm32f10x.h"
|
||||||
|
|
||||||
/**********************************************************************************************************************/
|
/**********************************************************************************************************************/
|
||||||
void delay_ms(uint16_t t); //using timer to delay time, max delay time is 1864ms
|
void delay_ms(uint16_t t); // using timer to delay time, max delay time is 1864ms
|
||||||
void delay_us(uint16_t t); //using timer to delay time, max delay time is 1864ms
|
void delay_us(uint16_t t); // using timer to delay time, max delay time is 1864ms
|
||||||
/**********************************************************************************************************************/
|
/**********************************************************************************************************************/
|
||||||
void PB_System_Timer_Init(void); //Initialize the Time measurement system
|
void PB_System_Timer_Init(void); // Initialize the Time measurement system
|
||||||
float GetSystemTime(void); //Return the current time(us), max:281474976s--->3257.8 days
|
float getSystemTime(void); // Return the current time(us), max:281474976s--->3257.8 days
|
||||||
float PB_Get_Dtime(void); // Return the time difference(us). max:655s
|
float getDeltaTime(void); // Return the time difference(us). max:655s
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -126,7 +126,7 @@ float PB_Get_Encode_TIM2(void)
|
||||||
return cnt;
|
return cnt;
|
||||||
}
|
}
|
||||||
|
|
||||||
float PB_Get_Encode_TIM3(void)
|
float Get_EncoderTIM3(void)
|
||||||
{
|
{
|
||||||
float cnt;
|
float cnt;
|
||||||
cnt = (float)((uint16_t)30000) - (float)((uint16_t)(TIM3->CNT)) ;
|
cnt = (float)((uint16_t)30000) - (float)((uint16_t)(TIM3->CNT)) ;
|
||||||
|
|
@ -134,7 +134,7 @@ float PB_Get_Encode_TIM3(void)
|
||||||
return cnt;
|
return cnt;
|
||||||
}
|
}
|
||||||
|
|
||||||
float PB_Get_Encode_TIM4(void)
|
float Get_EncoderTIM4(void)
|
||||||
{
|
{
|
||||||
float cnt;
|
float cnt;
|
||||||
cnt = (float)((uint16_t)30000) - (float)((uint16_t)(TIM4->CNT)) ;
|
cnt = (float)((uint16_t)30000) - (float)((uint16_t)(TIM4->CNT)) ;
|
||||||
|
|
|
||||||
|
|
@ -9,8 +9,8 @@ extern "C" {
|
||||||
|
|
||||||
void Encoder_Init(TIM_TypeDef* TIMx , unsigned char GPIO_AF);
|
void Encoder_Init(TIM_TypeDef* TIMx , unsigned char GPIO_AF);
|
||||||
float PB_Get_Encode_TIM5(void);//What you get here is the total angle.
|
float PB_Get_Encode_TIM5(void);//What you get here is the total angle.
|
||||||
float GetEncoderTIM4(void);
|
float Get_EncoderTIM4(void);
|
||||||
float GetEncoderTIM3(void);
|
float Get_EncoderTIM3(void);
|
||||||
float PB_Get_Encode_TIM2(void);
|
float PB_Get_Encode_TIM2(void);
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
|
|
|
||||||
|
|
@ -251,7 +251,7 @@ static char *itoa(int value, char *string, int radix)
|
||||||
|
|
||||||
} /* NCL_Itoa */
|
} /* NCL_Itoa */
|
||||||
|
|
||||||
void PB_USART_printf(USART_TypeDef* USARTx, uint8_t *Data,...)
|
void USART_printf(USART_TypeDef* USARTx, uint8_t *Data,...)
|
||||||
{
|
{
|
||||||
const char *s;
|
const char *s;
|
||||||
int d;
|
int d;
|
||||||
|
|
|
||||||
|
|
@ -12,7 +12,7 @@ extern "C" {
|
||||||
//Initilaize the serial, First Parameter:USART1,USART2,USART3; 2nd Para:bits rate; 3rd: IO reuse
|
//Initilaize the serial, First Parameter:USART1,USART2,USART3; 2nd Para:bits rate; 3rd: IO reuse
|
||||||
void PB_USART_Init(uint8_t USART_Channel ,unsigned int BaudRate,unsigned char GPIO_AF);
|
void PB_USART_Init(uint8_t USART_Channel ,unsigned int BaudRate,unsigned char GPIO_AF);
|
||||||
void PB_USART_Put_Char(uint8_t USART_Channel , unsigned char Tx_Byte); //USARTx to print 1 byte
|
void PB_USART_Put_Char(uint8_t USART_Channel , unsigned char Tx_Byte); //USARTx to print 1 byte
|
||||||
void PB_USART_printf(USART_TypeDef* USARTx, uint8_t *Data, ...); //format output as printf in C standard library
|
void USART_printf(USART_TypeDef* USARTx, uint8_t *Data, ...); //format output as printf in C standard library
|
||||||
|
|
||||||
//int fputc(int ch, FILE *f);
|
//int fputc(int ch, FILE *f);
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -311,7 +311,7 @@ void Board_STM32::motor_pwm(unsigned char num, long pwm_value)
|
||||||
|
|
||||||
unsigned long Board_STM32::getTickCount()
|
unsigned long Board_STM32::getTickCount()
|
||||||
{
|
{
|
||||||
return GetSystemTime() / 1000;
|
return getSystemTime() / 1000;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Board_STM32::encoder_init(unsigned char motor_id)
|
void Board_STM32::encoder_init(unsigned char motor_id)
|
||||||
|
|
@ -326,9 +326,9 @@ void Board_STM32::encoder_init(unsigned char motor_id)
|
||||||
long Board_STM32::getEncoderCount(unsigned char motorId)
|
long Board_STM32::getEncoderCount(unsigned char motorId)
|
||||||
{
|
{
|
||||||
if (motorId == MOTOR_1) {
|
if (motorId == MOTOR_1) {
|
||||||
return GetEncoderTIM3();
|
return Get_EncoderTIM3();
|
||||||
} else if (motorId == MOTOR_2) {
|
} else if (motorId == MOTOR_2) {
|
||||||
return GetEncoderTIM4();
|
return Get_EncoderTIM4();
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -4,9 +4,9 @@
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
#if DEBUG_ENABLE
|
#if DEBUG_ENABLE
|
||||||
#define pb_printf(format,...) printf("" __FILE__ ":%d - " format "\r\n", __LINE__, ##__VA_ARGS__)
|
#define log(format,...) printf("" __FILE__ ":%d - " format "\r\n", __LINE__, ##__VA_ARGS__)
|
||||||
#else
|
#else
|
||||||
#define pb_printf(format,...)
|
#define log(format,...)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -2,7 +2,7 @@
|
||||||
#include "board.h"
|
#include "board.h"
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
Data_holder::Data_holder()
|
DataHolder::DataHolder()
|
||||||
{
|
{
|
||||||
strncpy((char*)&firmware_info.version, FW_VERSION, strlen(FW_VERSION)>sizeof(firmware_info.version)?sizeof(firmware_info.version):strlen(FW_VERSION));
|
strncpy((char*)&firmware_info.version, FW_VERSION, strlen(FW_VERSION)>sizeof(firmware_info.version)?sizeof(firmware_info.version):strlen(FW_VERSION));
|
||||||
sprintf(firmware_info.time, "%s", "20220301");
|
sprintf(firmware_info.time, "%s", "20220301");
|
||||||
|
|
@ -13,7 +13,7 @@ Data_holder::Data_holder()
|
||||||
memset(imu_data, 0, sizeof(imu_data));
|
memset(imu_data, 0, sizeof(imu_data));
|
||||||
}
|
}
|
||||||
|
|
||||||
void Data_holder::load_parameter()
|
void DataHolder::loadParameter()
|
||||||
{
|
{
|
||||||
Board::get()->get_config((unsigned char*)¶meter, sizeof(parameter));
|
Board::get()->get_config((unsigned char*)¶meter, sizeof(parameter));
|
||||||
|
|
||||||
|
|
@ -30,7 +30,7 @@ void Data_holder::load_parameter()
|
||||||
parameter.params.model_type = MODEL_TYPE_2WD_DIFF; // in stm32f1 fix 2wd
|
parameter.params.model_type = MODEL_TYPE_2WD_DIFF; // in stm32f1 fix 2wd
|
||||||
}
|
}
|
||||||
|
|
||||||
void Data_holder::save_parameter()
|
void DataHolder::save_parameter()
|
||||||
{
|
{
|
||||||
Board::get()->set_config((unsigned char*)¶meter, sizeof(parameter));
|
Board::get()->set_config((unsigned char*)¶meter, sizeof(parameter));
|
||||||
parameter.params.model_type = MODEL_TYPE_2WD_DIFF; // in stm32f1 fix 2wd
|
parameter.params.model_type = MODEL_TYPE_2WD_DIFF; // in stm32f1 fix 2wd
|
||||||
|
|
|
||||||
|
|
@ -1,5 +1,5 @@
|
||||||
#ifndef DATA_HOLDER_H_
|
#ifndef DataHolder_H_
|
||||||
#define DATA_HOLDER_H_
|
#define DataHolder_H_
|
||||||
|
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
|
|
@ -82,20 +82,20 @@ struct Robot_pwm_value
|
||||||
short value[4];
|
short value[4];
|
||||||
};
|
};
|
||||||
|
|
||||||
class Data_holder
|
class DataHolder
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
static Data_holder* get() {
|
static DataHolder* get() {
|
||||||
static Data_holder dh;
|
static DataHolder dh;
|
||||||
return &dh;
|
return &dh;
|
||||||
}
|
}
|
||||||
|
|
||||||
void load_parameter();
|
void loadParameter();
|
||||||
|
|
||||||
void save_parameter();
|
void save_parameter();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
Data_holder();
|
DataHolder();
|
||||||
public:
|
public:
|
||||||
struct Robot_firmware firmware_info;//版本信息
|
struct Robot_firmware firmware_info;//版本信息
|
||||||
struct Robot_parameter parameter; //参数
|
struct Robot_parameter parameter; //参数
|
||||||
|
|
|
||||||
|
|
@ -15,7 +15,7 @@ bool GY65::init()
|
||||||
|
|
||||||
if (!mpu6050.testConnection()) {
|
if (!mpu6050.testConnection()) {
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
pb_printf("MPU6050 NOT FOUND!");
|
log("MPU6050 NOT FOUND!");
|
||||||
#endif
|
#endif
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
@ -26,14 +26,14 @@ bool GY65::init()
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
uint8_t buffer=0;
|
uint8_t buffer=0;
|
||||||
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, &buffer);
|
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, &buffer);
|
||||||
pb_printf("MPU6050_RA_PWR_MGMT_1=%d", buffer);
|
log("MPU6050_RA_PWR_MGMT_1=%d", buffer);
|
||||||
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, &buffer);
|
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, &buffer);
|
||||||
pb_printf("MPU6050_RA_GYRO_CONFIG=%d", buffer);
|
log("MPU6050_RA_GYRO_CONFIG=%d", buffer);
|
||||||
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, &buffer);
|
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, &buffer);
|
||||||
pb_printf("MPU6050_RA_ACCEL_CONFIG=%d", buffer);
|
log("MPU6050_RA_ACCEL_CONFIG=%d", buffer);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
pb_printf("GY65 INIT SUCCESS!");
|
log("GY65 INIT SUCCESS!");
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
@ -43,7 +43,7 @@ void GY65::get_data(float* imu_data)
|
||||||
mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
|
mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
|
||||||
|
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
pb_printf("[%d %d %d] [%d %d %d]", ax, ay, az, gx, gy, gz);
|
log("[%d %d %d] [%d %d %d]", ax, ay, az, gx, gy, gz);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
imu_data[0] = ax*ADX_SCALE;
|
imu_data[0] = ax*ADX_SCALE;
|
||||||
|
|
|
||||||
|
|
@ -15,7 +15,7 @@ bool GY85::init()
|
||||||
|
|
||||||
if (!accel.testConnection()) {
|
if (!accel.testConnection()) {
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
pb_printf("ADXL345 NOT FOUND!");
|
log("ADXL345 NOT FOUND!");
|
||||||
#endif
|
#endif
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
@ -34,16 +34,16 @@ bool GY85::init()
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
uint8_t buffer=0;
|
uint8_t buffer=0;
|
||||||
Board::get()->i2c_read_byte(ADXL345_DEFAULT_ADDRESS, ADXL345_RA_POWER_CTL, &buffer);
|
Board::get()->i2c_read_byte(ADXL345_DEFAULT_ADDRESS, ADXL345_RA_POWER_CTL, &buffer);
|
||||||
pb_printf("ADXL345_RA_POWER_CTL=%d", buffer);
|
log("ADXL345_RA_POWER_CTL=%d", buffer);
|
||||||
Board::get()->i2c_read_byte(ADXL345_DEFAULT_ADDRESS, ADXL345_RA_DATA_FORMAT, &buffer);
|
Board::get()->i2c_read_byte(ADXL345_DEFAULT_ADDRESS, ADXL345_RA_DATA_FORMAT, &buffer);
|
||||||
pb_printf("ADXL345_RA_DATA_FORMAT=%d", buffer);
|
log("ADXL345_RA_DATA_FORMAT=%d", buffer);
|
||||||
Board::get()->i2c_read_byte(ADXL345_DEFAULT_ADDRESS, ADXL345_RA_BW_RATE, &buffer);
|
Board::get()->i2c_read_byte(ADXL345_DEFAULT_ADDRESS, ADXL345_RA_BW_RATE, &buffer);
|
||||||
pb_printf("ADXL345_RA_BW_RATE=%d", buffer);
|
log("ADXL345_RA_BW_RATE=%d", buffer);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (!gyro.testConnection()) {
|
if (!gyro.testConnection()) {
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
pb_printf("ITG3205 NOT FOUND!");
|
log("ITG3205 NOT FOUND!");
|
||||||
#endif
|
#endif
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
@ -62,16 +62,16 @@ bool GY85::init()
|
||||||
|
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
Board::get()->i2c_read_byte(ITG3200_DEFAULT_ADDRESS, ITG3200_RA_PWR_MGM, &buffer);
|
Board::get()->i2c_read_byte(ITG3200_DEFAULT_ADDRESS, ITG3200_RA_PWR_MGM, &buffer);
|
||||||
pb_printf("ITG3200_RA_PWR_MGM=%d", buffer);
|
log("ITG3200_RA_PWR_MGM=%d", buffer);
|
||||||
Board::get()->i2c_read_byte(ITG3200_DEFAULT_ADDRESS, ITG3200_RA_DLPF_FS, &buffer);
|
Board::get()->i2c_read_byte(ITG3200_DEFAULT_ADDRESS, ITG3200_RA_DLPF_FS, &buffer);
|
||||||
pb_printf("ITG3200_RA_DLPF_FS=%d", buffer);
|
log("ITG3200_RA_DLPF_FS=%d", buffer);
|
||||||
Board::get()->i2c_read_byte(ITG3200_DEFAULT_ADDRESS, ITG3200_RA_SMPLRT_DIV, &buffer);
|
Board::get()->i2c_read_byte(ITG3200_DEFAULT_ADDRESS, ITG3200_RA_SMPLRT_DIV, &buffer);
|
||||||
pb_printf("ITG3200_RA_SMPLRT_DIV=%d", buffer);
|
log("ITG3200_RA_SMPLRT_DIV=%d", buffer);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (!mag.testConnection()) {
|
if (!mag.testConnection()) {
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
pb_printf("HMC5883L NOT FOUND!");
|
log("HMC5883L NOT FOUND!");
|
||||||
#endif
|
#endif
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
@ -89,14 +89,14 @@ bool GY85::init()
|
||||||
|
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_CONFIG_B, &buffer);
|
Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_CONFIG_B, &buffer);
|
||||||
pb_printf("HMC5883L_RA_CONFIG_B=%d", buffer);
|
log("HMC5883L_RA_CONFIG_B=%d", buffer);
|
||||||
Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_CONFIG_A, &buffer);
|
Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_CONFIG_A, &buffer);
|
||||||
pb_printf("HMC5883L_RA_CONFIG_A=%d", buffer);
|
log("HMC5883L_RA_CONFIG_A=%d", buffer);
|
||||||
Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_MODE, &buffer);
|
Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_MODE, &buffer);
|
||||||
pb_printf("HMC5883L_RA_MODE=%d", buffer);
|
log("HMC5883L_RA_MODE=%d", buffer);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
pb_printf("GY85 INIT SUCCESS!");
|
log("GY85 INIT SUCCESS!");
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
@ -119,7 +119,7 @@ void GY85::get_data(float* imu_data)
|
||||||
}
|
}
|
||||||
i++;
|
i++;
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
pb_printf("[%d %d %d] [%d %d %d] [%d %d %d]", ax, ay, az, gx, gy, gz, mx, my, mz);
|
log("[%d %d %d] [%d %d %d] [%d %d %d]", ax, ay, az, gx, gy, gz, mx, my, mz);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
imu_data[0] = ax*ADXL345_SCALE;
|
imu_data[0] = ax*ADXL345_SCALE;
|
||||||
|
|
|
||||||
|
|
@ -13,13 +13,13 @@ bool GY87::init()
|
||||||
{
|
{
|
||||||
delay_ms(500);
|
delay_ms(500);
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
pb_printf("GY87::init");
|
log("GY87::init");
|
||||||
#endif
|
#endif
|
||||||
Board::get()->i2c_init();
|
Board::get()->i2c_init();
|
||||||
|
|
||||||
if (!mpu6050.testConnection()) {
|
if (!mpu6050.testConnection()) {
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
pb_printf("MPU6050 NOT FOUND!");
|
log("MPU6050 NOT FOUND!");
|
||||||
#endif
|
#endif
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
@ -30,16 +30,16 @@ bool GY87::init()
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
uint8_t buffer=0;
|
uint8_t buffer=0;
|
||||||
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, &buffer);
|
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, &buffer);
|
||||||
pb_printf("MPU6050_RA_PWR_MGMT_1=%d", buffer);
|
log("MPU6050_RA_PWR_MGMT_1=%d", buffer);
|
||||||
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, &buffer);
|
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, &buffer);
|
||||||
pb_printf("MPU6050_RA_GYRO_CONFIG=%d", buffer);
|
log("MPU6050_RA_GYRO_CONFIG=%d", buffer);
|
||||||
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, &buffer);
|
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, &buffer);
|
||||||
pb_printf("MPU6050_RA_ACCEL_CONFIG=%d", buffer);
|
log("MPU6050_RA_ACCEL_CONFIG=%d", buffer);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (!mag.testConnection()) {
|
if (!mag.testConnection()) {
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
pb_printf("HMC5883L NOT FOUND!");
|
log("HMC5883L NOT FOUND!");
|
||||||
#endif
|
#endif
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
@ -57,14 +57,14 @@ bool GY87::init()
|
||||||
|
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_CONFIG_B, &buffer);
|
Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_CONFIG_B, &buffer);
|
||||||
pb_printf("HMC5883L_RA_CONFIG_B=%d", buffer);
|
log("HMC5883L_RA_CONFIG_B=%d", buffer);
|
||||||
Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_CONFIG_A, &buffer);
|
Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_CONFIG_A, &buffer);
|
||||||
pb_printf("HMC5883L_RA_CONFIG_A=%d", buffer);
|
log("HMC5883L_RA_CONFIG_A=%d", buffer);
|
||||||
Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_MODE, &buffer);
|
Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_MODE, &buffer);
|
||||||
pb_printf("HMC5883L_RA_MODE=%d", buffer);
|
log("HMC5883L_RA_MODE=%d", buffer);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
pb_printf("GY87 INIT SUCCESS!");
|
log("GY87 INIT SUCCESS!");
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
@ -83,7 +83,7 @@ void GY87::get_data(float* imu_data)
|
||||||
i++;
|
i++;
|
||||||
|
|
||||||
#if IMU_DEBUG_ENABLE
|
#if IMU_DEBUG_ENABLE
|
||||||
// pb_printf("[%d %d %d] [%d %d %d] [%d %d %d]", ax, ay, az, gx, gy, gz, mx, my, mz);
|
// log("[%d %d %d] [%d %d %d] [%d %d %d]", ax, ay, az, gx, gy, gz, mx, my, mz);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
imu_data[0] = ax*ADX_SCALE;
|
imu_data[0] = ax*ADX_SCALE;
|
||||||
|
|
|
||||||
|
|
@ -74,6 +74,6 @@ uint8_t MPU6050::getDeviceID()
|
||||||
{
|
{
|
||||||
buffer[0]=0xff;
|
buffer[0]=0xff;
|
||||||
Board::get()->i2c_read_bits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
|
Board::get()->i2c_read_bits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
|
||||||
pb_printf("MPU6050 id: 0x%02x", buffer[0]);
|
log("MPU6050 id: 0x%02x", buffer[0]);
|
||||||
return buffer[0];
|
return buffer[0];
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -12,28 +12,28 @@ Joystick::Joystick()
|
||||||
|
|
||||||
bool Joystick::init()
|
bool Joystick::init()
|
||||||
{
|
{
|
||||||
pb_printf("init joystick");
|
log("init joystick");
|
||||||
//PS2_SetInit();
|
//PS2_SetInit();
|
||||||
#if 0
|
#if 0
|
||||||
unsigned long start = Board::get()->get_tick_count();
|
unsigned long start = Board::get()->getTickCount();
|
||||||
while(Board::get()->get_tick_count()-start>500){
|
while(Board::get()->getTickCount()-start>500){
|
||||||
;
|
;
|
||||||
}
|
}
|
||||||
error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
|
error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
|
||||||
if (error == 0)
|
if (error == 0)
|
||||||
{
|
{
|
||||||
pb_printf("Teleop start");
|
log("Teleop start");
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (error == 1)
|
if (error == 1)
|
||||||
pb_printf("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
|
log("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
|
||||||
else if (error == 2)
|
else if (error == 2)
|
||||||
pb_printf("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
|
log("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
|
||||||
else if (error == 3)
|
else if (error == 3)
|
||||||
pb_printf("Controller refusing to enter Pressures mode, may not support it. ");
|
log("Controller refusing to enter Pressures mode, may not support it. ");
|
||||||
else
|
else
|
||||||
pb_printf("Teleop err");
|
log("Teleop err");
|
||||||
#endif
|
#endif
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
@ -43,66 +43,66 @@ void Joystick::test()
|
||||||
{
|
{
|
||||||
#ifdef JOYSTICK_TEST
|
#ifdef JOYSTICK_TEST
|
||||||
|
|
||||||
unsigned char key=PS2_DataKey();
|
unsigned char key = PS2_DataKey();
|
||||||
|
|
||||||
//有按键按下
|
//有按键按下
|
||||||
if(key!=0) {
|
if(key!=0) {
|
||||||
switch (key) {
|
switch (key) {
|
||||||
case PSB_SELECT:
|
case PSB_SELECT:
|
||||||
pb_printf("SELECT");
|
log("SELECT");
|
||||||
break;
|
break;
|
||||||
case PSB_L3:
|
case PSB_L3:
|
||||||
pb_printf("L3");
|
log("L3");
|
||||||
break;
|
break;
|
||||||
case PSB_R3:
|
case PSB_R3:
|
||||||
pb_printf("R3");
|
log("R3");
|
||||||
break;
|
break;
|
||||||
case PSB_START:
|
case PSB_START:
|
||||||
pb_printf("START");
|
log("START");
|
||||||
break;
|
break;
|
||||||
case PSB_PAD_UP:
|
case PSB_PAD_UP:
|
||||||
pb_printf("UP");
|
log("UP");
|
||||||
break;
|
break;
|
||||||
case PSB_PAD_RIGHT:
|
case PSB_PAD_RIGHT:
|
||||||
pb_printf("RIGHT");
|
log("RIGHT");
|
||||||
break;
|
break;
|
||||||
case PSB_PAD_DOWN:
|
case PSB_PAD_DOWN:
|
||||||
pb_printf("DOWN");
|
log("DOWN");
|
||||||
break;
|
break;
|
||||||
case PSB_PAD_LEFT:
|
case PSB_PAD_LEFT:
|
||||||
pb_printf("LEFT");
|
log("LEFT");
|
||||||
break;
|
break;
|
||||||
case PSB_L2:
|
case PSB_L2:
|
||||||
pb_printf("L2");
|
log("L2");
|
||||||
break;
|
break;
|
||||||
case PSB_R2:
|
case PSB_R2:
|
||||||
pb_printf("R2");
|
log("R2");
|
||||||
break;
|
break;
|
||||||
case PSB_L1:
|
case PSB_L1:
|
||||||
pb_printf("L1");
|
log("L1");
|
||||||
break;
|
break;
|
||||||
case PSB_R1:
|
case PSB_R1:
|
||||||
pb_printf("R1");
|
log("R1");
|
||||||
break;
|
break;
|
||||||
case PSB_TRIANGLE:
|
case PSB_TRIANGLE:
|
||||||
pb_printf("TRIANGLE");
|
log("TRIANGLE");
|
||||||
break;
|
break;
|
||||||
case PSB_CIRCLE:
|
case PSB_CIRCLE:
|
||||||
pb_printf("CIRCLE");
|
log("CIRCLE");
|
||||||
break;
|
break;
|
||||||
case PSB_CROSS:
|
case PSB_CROSS:
|
||||||
pb_printf("CROSS");
|
log("CROSS");
|
||||||
break;
|
break;
|
||||||
case PSB_SQUARE:
|
case PSB_SQUARE:
|
||||||
pb_printf("SQUARE");
|
log("SQUARE");
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
pb_printf("UNKNOWN");
|
log("UNKNOWN");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
pb_printf("(need to switch mode) %5d %5d %5d %5d",PS2_AnologData(PSS_LX),PS2_AnologData(PSS_LY), PS2_AnologData(PSS_RX),PS2_AnologData(PSS_RY) );
|
log("(need to switch mode) %5d %5d %5d %5d",PS2_AnologData(PSS_LX),PS2_AnologData(PSS_LY), PS2_AnologData(PSS_RX),PS2_AnologData(PSS_RY) );
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -117,13 +117,13 @@ bool Joystick::check(short& liner_x, short liner_y, short& angular_z){
|
||||||
bool Joystick::holonomic_check(short& liner_x, short liner_y, short& angular_z){
|
bool Joystick::holonomic_check(short& liner_x, short liner_y, short& angular_z){
|
||||||
bool rtn = false;
|
bool rtn = false;
|
||||||
|
|
||||||
unsigned char key=PS2_DataKey();
|
unsigned char key = PS2_DataKey();
|
||||||
|
|
||||||
liner_y = liner_y;
|
liner_y = liner_y;
|
||||||
//up down left right for liner x, y
|
//up down left right for liner x, y
|
||||||
if (key == PSB_PAD_UP) {
|
if (key == PSB_PAD_UP) {
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("UP");
|
log("UP");
|
||||||
#endif
|
#endif
|
||||||
liner_x = MAX_LINER_X;
|
liner_x = MAX_LINER_X;
|
||||||
rtn = true;
|
rtn = true;
|
||||||
|
|
@ -131,7 +131,7 @@ bool Joystick::holonomic_check(short& liner_x, short liner_y, short& angular_z){
|
||||||
|
|
||||||
if(key == PSB_PAD_DOWN) {
|
if(key == PSB_PAD_DOWN) {
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("DOWN");
|
log("DOWN");
|
||||||
#endif
|
#endif
|
||||||
liner_x = -MAX_LINER_X;
|
liner_x = -MAX_LINER_X;
|
||||||
rtn = true;
|
rtn = true;
|
||||||
|
|
@ -139,7 +139,7 @@ bool Joystick::holonomic_check(short& liner_x, short liner_y, short& angular_z){
|
||||||
|
|
||||||
if(key == PSB_PAD_RIGHT) {
|
if(key == PSB_PAD_RIGHT) {
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("RIGHT");
|
log("RIGHT");
|
||||||
#endif
|
#endif
|
||||||
liner_y = MAX_LINER_Y;
|
liner_y = MAX_LINER_Y;
|
||||||
rtn = true;
|
rtn = true;
|
||||||
|
|
@ -147,7 +147,7 @@ bool Joystick::holonomic_check(short& liner_x, short liner_y, short& angular_z){
|
||||||
|
|
||||||
if(key == PSB_PAD_LEFT) {
|
if(key == PSB_PAD_LEFT) {
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("LEFT");
|
log("LEFT");
|
||||||
#endif
|
#endif
|
||||||
liner_y = -MAX_LINER_Y;
|
liner_y = -MAX_LINER_Y;
|
||||||
rtn = true;
|
rtn = true;
|
||||||
|
|
@ -156,7 +156,7 @@ bool Joystick::holonomic_check(short& liner_x, short liner_y, short& angular_z){
|
||||||
//triangle square circle cross for angular z
|
//triangle square circle cross for angular z
|
||||||
if (key == PSB_SQUARE) {
|
if (key == PSB_SQUARE) {
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("SQUARE");
|
log("SQUARE");
|
||||||
#endif
|
#endif
|
||||||
angular_z = MAX_ANGULAR_Z;
|
angular_z = MAX_ANGULAR_Z;
|
||||||
rtn = true;
|
rtn = true;
|
||||||
|
|
@ -164,7 +164,7 @@ bool Joystick::holonomic_check(short& liner_x, short liner_y, short& angular_z){
|
||||||
|
|
||||||
if(key == PSB_CIRCLE) {
|
if(key == PSB_CIRCLE) {
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("CIRCLE");
|
log("CIRCLE");
|
||||||
#endif
|
#endif
|
||||||
angular_z = -MAX_ANGULAR_Z;
|
angular_z = -MAX_ANGULAR_Z;
|
||||||
rtn = true;
|
rtn = true;
|
||||||
|
|
@ -172,25 +172,25 @@ bool Joystick::holonomic_check(short& liner_x, short liner_y, short& angular_z){
|
||||||
|
|
||||||
if (key == PSB_L1) { //print stick values if either is TRUE
|
if (key == PSB_L1) { //print stick values if either is TRUE
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("Stick Values:");
|
log("Stick Values:");
|
||||||
pb_printf("%d", PS2_AnologData(PSS_LY));
|
log("%d", PS2_AnologData(PSS_LY));
|
||||||
pb_printf(",");
|
log(",");
|
||||||
pb_printf("%d", PS2_AnologData(PSS_LX));
|
log("%d", PS2_AnologData(PSS_LX));
|
||||||
pb_printf(",");
|
log(",");
|
||||||
pb_printf("%d", PS2_AnologData(PSS_RY));
|
log("%d", PS2_AnologData(PSS_RY));
|
||||||
pb_printf(",");
|
log(",");
|
||||||
pb_printf("%d", PS2_AnologData(PSS_RX));
|
log("%d", PS2_AnologData(PSS_RX));
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255 &&
|
if (PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255 &&
|
||||||
PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255) {
|
PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255) {
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("switch mode for use rocker");
|
log("switch mode for use rocker");
|
||||||
#endif
|
#endif
|
||||||
} else {
|
} else {
|
||||||
liner_x = ((255.0/2)-PS2_AnologData(PSS_LY))/(255.0/2)*MAX_LINER_X;
|
liner_x = ((255.0/2) - PS2_AnologData(PSS_LY)) / (255.0/2) * MAX_LINER_X;
|
||||||
liner_y = ((255.0/2)-PS2_AnologData(PSS_LX))/(255.0/2)*MAX_LINER_Y;
|
liner_y = ((255.0/2) - PS2_AnologData(PSS_LX)) / (255.0/2) * MAX_LINER_Y;
|
||||||
angular_z = ((255.0/2)-PS2_AnologData(PSS_RX))/(255.0/2)*MAX_ANGULAR_Z;
|
angular_z = ((255.0/2) - PS2_AnologData(PSS_RX)) / (255.0/2) * MAX_ANGULAR_Z;
|
||||||
rtn = true;
|
rtn = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -210,7 +210,7 @@ bool Joystick::nonholonomic_check(short& liner_x, short liner_y, short& angular_
|
||||||
//will be TRUE if button was JUST pressed
|
//will be TRUE if button was JUST pressed
|
||||||
if (key == PSB_PAD_UP) {
|
if (key == PSB_PAD_UP) {
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("Up held this hard: ");
|
log("Up held this hard: ");
|
||||||
#endif
|
#endif
|
||||||
liner_x = MAX_LINER_X;
|
liner_x = MAX_LINER_X;
|
||||||
rtn = true;
|
rtn = true;
|
||||||
|
|
@ -218,7 +218,7 @@ bool Joystick::nonholonomic_check(short& liner_x, short liner_y, short& angular_
|
||||||
|
|
||||||
if(key == PSB_PAD_RIGHT) {
|
if(key == PSB_PAD_RIGHT) {
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("Right held this hard: ");
|
log("Right held this hard: ");
|
||||||
#endif
|
#endif
|
||||||
angular_z = MAX_ANGULAR_Z;
|
angular_z = MAX_ANGULAR_Z;
|
||||||
rtn = true;
|
rtn = true;
|
||||||
|
|
@ -226,7 +226,7 @@ bool Joystick::nonholonomic_check(short& liner_x, short liner_y, short& angular_
|
||||||
|
|
||||||
if(key == PSB_PAD_LEFT) {
|
if(key == PSB_PAD_LEFT) {
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("LEFT held this hard: ");
|
log("LEFT held this hard: ");
|
||||||
#endif
|
#endif
|
||||||
angular_z = -MAX_ANGULAR_Z;
|
angular_z = -MAX_ANGULAR_Z;
|
||||||
rtn = true;
|
rtn = true;
|
||||||
|
|
@ -234,7 +234,7 @@ bool Joystick::nonholonomic_check(short& liner_x, short liner_y, short& angular_
|
||||||
|
|
||||||
if(key == PSB_PAD_DOWN) {
|
if(key == PSB_PAD_DOWN) {
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("DOWN held this hard: ");
|
log("DOWN held this hard: ");
|
||||||
#endif
|
#endif
|
||||||
liner_x = -MAX_LINER_X;
|
liner_x = -MAX_LINER_X;
|
||||||
rtn = true;
|
rtn = true;
|
||||||
|
|
@ -242,20 +242,20 @@ bool Joystick::nonholonomic_check(short& liner_x, short liner_y, short& angular_
|
||||||
|
|
||||||
if (key == PSB_L1) { //print stick values if either is TRUE
|
if (key == PSB_L1) { //print stick values if either is TRUE
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("Stick Values:");
|
log("Stick Values:");
|
||||||
pb_printf("%d", PS2_AnologData(PSS_LY));
|
log("%d", PS2_AnologData(PSS_LY));
|
||||||
pb_printf(",");
|
log(",");
|
||||||
pb_printf("%d", PS2_AnologData(PSS_LX));
|
log("%d", PS2_AnologData(PSS_LX));
|
||||||
pb_printf(",");
|
log(",");
|
||||||
pb_printf("%d", PS2_AnologData(PSS_RY));
|
log("%d", PS2_AnologData(PSS_RY));
|
||||||
pb_printf(",");
|
log(",");
|
||||||
pb_printf("%d", PS2_AnologData(PSS_RX));
|
log("%d", PS2_AnologData(PSS_RX));
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255 &&
|
if (PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255 &&
|
||||||
PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255) {
|
PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255) {
|
||||||
#if JOYSTICK_DEBUG_ENABLE
|
#if JOYSTICK_DEBUG_ENABLE
|
||||||
pb_printf("switch mode for use rocker");
|
log("switch mode for use rocker");
|
||||||
#endif
|
#endif
|
||||||
} else {
|
} else {
|
||||||
liner_x = ((255.0/2)-PS2_AnologData(PSS_LY))/(255.0/2)*MAX_LINER_X;
|
liner_x = ((255.0/2)-PS2_AnologData(PSS_LY))/(255.0/2)*MAX_LINER_X;
|
||||||
|
|
|
||||||
|
|
@ -16,7 +16,7 @@ public:
|
||||||
Differential(float _wheel_radius, float _body_radius) : Model(_wheel_radius, _body_radius) {}
|
Differential(float _wheel_radius, float _body_radius) : Model(_wheel_radius, _body_radius) {}
|
||||||
|
|
||||||
//运动解算 把给到机器人的速度分解为各个轮子速度
|
//运动解算 把给到机器人的速度分解为各个轮子速度
|
||||||
virtual void motion_solver(float* robot_speed, float* motor_speed) {
|
virtual void motionSolver(float* robot_speed, float* motor_speed) {
|
||||||
// robot_speed[0] x robot_speed[1] y robot_speed[2] z
|
// robot_speed[0] x robot_speed[1] y robot_speed[2] z
|
||||||
motor_speed[0] = (-robot_speed[0] + robot_speed[2] * body_radius)/ wheel_radius;
|
motor_speed[0] = (-robot_speed[0] + robot_speed[2] * body_radius)/ wheel_radius;
|
||||||
motor_speed[1] = (robot_speed[0] + robot_speed[2] * body_radius) / wheel_radius;
|
motor_speed[1] = (robot_speed[0] + robot_speed[2] * body_radius) / wheel_radius;
|
||||||
|
|
|
||||||
|
|
@ -29,7 +29,7 @@ struct Model
|
||||||
~Model(){}
|
~Model(){}
|
||||||
|
|
||||||
//robot speed ------------> motor speed 运动解算 把给到机器人的速度分解为各个轮子速度
|
//robot speed ------------> motor speed 运动解算 把给到机器人的速度分解为各个轮子速度
|
||||||
virtual void motion_solver(float* robot_speed, float* motor_speed) = 0;
|
virtual void motionSolver(float* robot_speed, float* motor_speed) = 0;
|
||||||
|
|
||||||
//motor speed-------------> robot speed 反解算， 把各个轮子的速度转为机器人的速度 ，这里通过固定时间间隔转为里程
|
//motor speed-------------> robot speed 反解算， 把各个轮子的速度转为机器人的速度 ，这里通过固定时间间隔转为里程
|
||||||
//interval ms
|
//interval ms
|
||||||
|
|
|
||||||
|
|
@ -18,7 +18,7 @@ public:
|
||||||
Omni3(float _wheel_radius, float _body_radius) : Model(_wheel_radius, _body_radius) {}
|
Omni3(float _wheel_radius, float _body_radius) : Model(_wheel_radius, _body_radius) {}
|
||||||
|
|
||||||
//运动解算 把给到机器人的速度分解为各个轮子速度
|
//运动解算 把给到机器人的速度分解为各个轮子速度
|
||||||
void motion_solver(float* robot_speed, float* motor_speed) {
|
void motionSolver(float* robot_speed, float* motor_speed) {
|
||||||
// robot_speed[0] x robot_speed[1] y robot_speed[2] z
|
// robot_speed[0] x robot_speed[1] y robot_speed[2] z
|
||||||
motor_speed[0] = (robot_speed[1] + robot_speed[2] * body_radius)/ wheel_radius;
|
motor_speed[0] = (robot_speed[1] + robot_speed[2] * body_radius)/ wheel_radius;
|
||||||
motor_speed[1] = (-robot_speed[0]*sqrt_of_3*0.5 - robot_speed[1]*0.5 + robot_speed[2] * body_radius) / wheel_radius;
|
motor_speed[1] = (-robot_speed[0]*sqrt_of_3*0.5 - robot_speed[1]*0.5 + robot_speed[2] * body_radius) / wheel_radius;
|
||||||
|
|
|
||||||
|
|
@ -34,17 +34,17 @@ short PID::compute(float interval)
|
||||||
|
|
||||||
if (ki != 0) {
|
if (ki != 0) {
|
||||||
#if PID_DEBUG_OUTPUT
|
#if PID_DEBUG_OUTPUT
|
||||||
pb_printf("integra=%ld max_output=%ld %ld", long(integra*1000), long(-(max_output/ki*1000)), long(max_output/ki*1000));
|
log("integra=%ld max_output=%ld %ld", long(integra*1000), long(-(max_output/ki*1000)), long(max_output/ki*1000));
|
||||||
#endif
|
#endif
|
||||||
if (integra < -(max_output/ki)) {
|
if (integra < -(max_output/ki)) {
|
||||||
#if PID_DEBUG_OUTPUT
|
#if PID_DEBUG_OUTPUT
|
||||||
pb_printf("integra clear-");
|
log("integra clear-");
|
||||||
#endif
|
#endif
|
||||||
integra = -(max_output/ki);
|
integra = -(max_output/ki);
|
||||||
}
|
}
|
||||||
if (integra > max_output/ki) {
|
if (integra > max_output/ki) {
|
||||||
#if PID_DEBUG_OUTPUT
|
#if PID_DEBUG_OUTPUT
|
||||||
pb_printf("integra clear+");
|
log("integra clear+");
|
||||||
#endif
|
#endif
|
||||||
integra = max_output/ki;
|
integra = max_output/ki;
|
||||||
}
|
}
|
||||||
|
|
@ -58,7 +58,7 @@ short PID::compute(float interval)
|
||||||
val = max_output-1;
|
val = max_output-1;
|
||||||
|
|
||||||
#if PID_DEBUG_OUTPUT
|
#if PID_DEBUG_OUTPUT
|
||||||
pb_printf("error=%ld integra=%ld derivative=%ld val=%ld", long(error*1000), long(integra*1000), long(derivative*1000), long(val*1000));
|
log("error=%ld integra=%ld derivative=%ld val=%ld", long(error*1000), long(integra*1000), long(derivative*1000), long(val*1000));
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
return val;
|
return val;
|
||||||
|
|
|
||||||
|
|
@ -68,9 +68,9 @@ bool Simple_dataframe::dataParse()
|
||||||
{
|
{
|
||||||
MESSAGE_ID id = (MESSAGE_ID)active_rx_msg.head.msg_id;
|
MESSAGE_ID id = (MESSAGE_ID)active_rx_msg.head.msg_id;
|
||||||
|
|
||||||
//pb_printf("\r\ndata_parse:id=%d", id);
|
//log("\r\ndata_parse:id=%d", id);
|
||||||
|
|
||||||
Data_holder* dh = Data_holder::get();
|
DataHolder* dh = DataHolder::get();
|
||||||
//更新接收到的id执行响应操作，一般分为读取，写入，和执行动作3个操作
|
//更新接收到的id执行响应操作，一般分为读取，写入，和执行动作3个操作
|
||||||
switch (id) {
|
switch (id) {
|
||||||
case ID_GET_VERSION://读取版本
|
case ID_GET_VERSION://读取版本
|
||||||
|
|
|
||||||
|
|
@ -12,7 +12,7 @@ USART_transport::USART_transport(unsigned char num, unsigned long buad) : usart_
|
||||||
|
|
||||||
bool USART_transport::init()
|
bool USART_transport::init()
|
||||||
{
|
{
|
||||||
//pb_printf("port=%d %ld", usart_num, usart_buad);
|
//log("port=%d %ld", usart_num, usart_buad);
|
||||||
Board::get()->usart_init(usart_num, usart_buad);
|
Board::get()->usart_init(usart_num, usart_buad);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,36 +1,24 @@
|
||||||
#include "robot.h"
|
#include "robot.h"
|
||||||
#include "delay.h"
|
|
||||||
#include "board.h"
|
|
||||||
#include "print.h"
|
|
||||||
int main(void)
|
int main(void)
|
||||||
{
|
{
|
||||||
//初始化
|
//初始化
|
||||||
Robot::get()->init();
|
Robot::get()->Init();
|
||||||
|
|
||||||
while (1) {
|
while (1) {
|
||||||
// 检测接收的命令
|
// 检测接收的命令
|
||||||
Robot::get()->check_command();
|
Robot::get()->CheckCommand();
|
||||||
|
|
||||||
// 运动解算 把上位机下发的机器人的速度（线速度角速度）转为各个轮子的速度
|
// 运动解算 把上位机下发的机器人的速度（线速度角速度）转为各个轮子的速度
|
||||||
Robot::get()->do_kinmatics();
|
Robot::get()->DoKinmatics();
|
||||||
|
|
||||||
// 根据轮子的编码器计算得到机器人的里程计
|
// 根据轮子的编码器计算得到机器人的里程计
|
||||||
Robot::get()->calc_odom();
|
Robot::get()->CalcOdom();
|
||||||
|
|
||||||
// 读取imu的值
|
// 读取imu的值
|
||||||
Robot::get()->get_imu_data();
|
Robot::get()->GetImuData();
|
||||||
|
|
||||||
// 检测处理ps2的按键信息
|
// 检测处理ps2的按键信息
|
||||||
Robot::get()->check_joystick();
|
Robot::get()->CheckJoystick();
|
||||||
|
|
||||||
|
|
||||||
//// Board::get()->motor_pwm(0, 2479);
|
|
||||||
//// Board::get()->motor_pwm(1, -2107);
|
|
||||||
//
|
|
||||||
// int count_left = Board::get()->get_encoder_count(0);
|
|
||||||
// int count_right = Board::get()->get_encoder_count(1);
|
|
||||||
//
|
|
||||||
// pb_printf("left = %d, right = %d\n", count_left, count_right);
|
|
||||||
// delay_ms(10);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -61,7 +61,7 @@ Robot::Robot()
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void Robot::init()
|
void Robot::Init()
|
||||||
{
|
{
|
||||||
// board初始化
|
// board初始化
|
||||||
Board::get()->init();
|
Board::get()->init();
|
||||||
|
|
@ -71,52 +71,50 @@ void Robot::init()
|
||||||
Board::get()->usart_debug_init();
|
Board::get()->usart_debug_init();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// 加载参数， 该参数一般存储在flash或者eeprom
|
// 加载参数, 该参数一般存储在flash或者eeprom
|
||||||
Data_holder::get()->load_parameter();
|
DataHolder::get()->loadParameter();
|
||||||
|
|
||||||
PB_USART_printf(USART1, (uint8_t *)"Init\n");
|
log("RobotParameters: %d %d %d %d %d %d %d %d %d %d %d %d %d",
|
||||||
|
DataHolder::get()->parameter.params.wheel_diameter, DataHolder::get()->parameter.params.wheel_track, DataHolder::get()->parameter.params.encoder_resolution, DataHolder::get()->parameter.params.motor_ratio,
|
||||||
pb_printf("RobotParameters: %d %d %d %d %d %d %d %d %d %d %d %d %d",
|
DataHolder::get()->parameter.params.do_pid_interval, DataHolder::get()->parameter.params.kp, DataHolder::get()->parameter.params.ki, DataHolder::get()->parameter.params.kd, DataHolder::get()->parameter.params.ko,
|
||||||
Data_holder::get()->parameter.params.wheel_diameter, Data_holder::get()->parameter.params.wheel_track, Data_holder::get()->parameter.params.encoder_resolution, Data_holder::get()->parameter.params.motor_ratio,
|
DataHolder::get()->parameter.params.cmd_last_time, DataHolder::get()->parameter.params.max_v_liner_x, DataHolder::get()->parameter.params.max_v_liner_y, DataHolder::get()->parameter.params.max_v_angular_z);
|
||||||
Data_holder::get()->parameter.params.do_pid_interval, Data_holder::get()->parameter.params.kp, Data_holder::get()->parameter.params.ki, Data_holder::get()->parameter.params.kd, Data_holder::get()->parameter.params.ko,
|
|
||||||
Data_holder::get()->parameter.params.cmd_last_time, Data_holder::get()->parameter.params.max_v_liner_x, Data_holder::get()->parameter.params.max_v_liner_y, Data_holder::get()->parameter.params.max_v_angular_z);
|
|
||||||
|
|
||||||
#if IMU_ENABLE
|
#if IMU_ENABLE
|
||||||
// imu 初始化
|
// imu 初始化
|
||||||
init_imu();
|
init_imu();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
pb_printf("init_motor");
|
log("init_motor");
|
||||||
// 电机相关初始化
|
// 电机相关初始化
|
||||||
init_motor();
|
InitMotor();
|
||||||
|
|
||||||
pb_printf("init_trans");
|
log("init_trans");
|
||||||
// 通讯相关初始化
|
// 通讯相关初始化
|
||||||
init_trans();
|
InitTrans();
|
||||||
|
|
||||||
// joystick初始化
|
// joystick初始化
|
||||||
init_joystick();
|
InitJoystick();
|
||||||
|
|
||||||
pb_printf("pibot startup");
|
log("gebot init finish.");
|
||||||
}
|
}
|
||||||
|
|
||||||
void Robot::init_imu()
|
void Robot::init_imu()
|
||||||
{
|
{
|
||||||
#if IMU_ENABLE
|
#if IMU_ENABLE
|
||||||
if (Data_holder::get()->parameter.params.imu_type == IMU_TYPE_GY65) {
|
if (DataHolder::get()->parameter.params.imu_type == IMU_TYPE_GY65) {
|
||||||
pb_printf("imu gy65");
|
log("imu gy65");
|
||||||
static GY65 gy65;
|
static GY65 gy65;
|
||||||
imu = &gy65;
|
imu = &gy65;
|
||||||
} else if (Data_holder::get()->parameter.params.imu_type == IMU_TYPE_GY85) {
|
} else if (DataHolder::get()->parameter.params.imu_type == IMU_TYPE_GY85) {
|
||||||
pb_printf("imu gy85");
|
log("imu gy85");
|
||||||
static GY85 gy85;
|
static GY85 gy85;
|
||||||
imu = &gy85;
|
imu = &gy85;
|
||||||
} else if (Data_holder::get()->parameter.params.imu_type == IMU_TYPE_GY87) {
|
} else if (DataHolder::get()->parameter.params.imu_type == IMU_TYPE_GY87) {
|
||||||
pb_printf("imu gy87");
|
log("imu gy87");
|
||||||
static GY87 gy87;
|
static GY87 gy87;
|
||||||
imu = &gy87;
|
imu = &gy87;
|
||||||
} else {
|
} else {
|
||||||
pb_printf("imu default null driver");
|
log("imu default null driver");
|
||||||
imu = NULL;
|
imu = NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -126,7 +124,7 @@ void Robot::init_imu()
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void Robot::init_joystick()
|
void Robot::InitJoystick()
|
||||||
{
|
{
|
||||||
#if JOYSTICK_ENABLE
|
#if JOYSTICK_ENABLE
|
||||||
static Joystick joy;
|
static Joystick joy;
|
||||||
|
|
@ -136,50 +134,50 @@ void Robot::init_joystick()
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void Robot::init_motor()
|
void Robot::InitMotor()
|
||||||
{
|
{
|
||||||
//MOTOR_COUNT为电机数量 该宏定义在相应的机器人模型文件中， KinematicModels/differential.h
|
//MOTOR_COUNT为电机数量 该宏定义在相应的机器人模型文件中， KinematicModels/differential.h
|
||||||
#if MOTOR_COUNT>0 //电机1
|
#if MOTOR_COUNT>0 //电机1
|
||||||
// MOTOR_CONTROLLER在params.mk中定义 表示电机控制器类型
|
// MOTOR_CONTROLLER在params.mk中定义 表示电机控制器类型
|
||||||
// 当前使用的COMMON_CONTROLLER为2个io控制方向一个PWD控制速度类型的控制器
|
// 当前使用的COMMON_CONTROLLER为2个io控制方向一个PWD控制速度类型的控制器
|
||||||
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
|
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
|
||||||
static CommonMotorController motor1(MOTOR_1, MAX_PWM_VALUE, (Data_holder::get()->parameter.params.motor_nonexchange_flag & MOTOR_ENCODER_1_FLAG)==0); // MOTOR1_REVERS标识电机方向反向,等同于电机线交换
|
static CommonMotorController motor1(MOTOR_1, MAX_PWM_VALUE, (DataHolder::get()->parameter.params.motor_nonexchange_flag & MOTOR_ENCODER_1_FLAG)==0); // MOTOR1_REVERS标识电机方向反向,等同于电机线交换
|
||||||
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
|
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
|
||||||
static AFSMotorController motor1(MOTOR_1_PORT_NUM, MAX_PWM_VALUE);
|
static AFSMotorController motor1(MOTOR_1_PORT_NUM, MAX_PWM_VALUE);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// 定义使用的编码器 传入参数为编码器的AB GPIO引脚 定义在params.mk中
|
// 定义使用的编码器 传入参数为编码器的AB GPIO引脚 定义在params.mk中
|
||||||
static EncoderImp encoder1(MOTOR_1, (Data_holder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_1_FLAG)==0);
|
static EncoderImp encoder1(MOTOR_1, (DataHolder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_1_FLAG)==0);
|
||||||
|
|
||||||
// 定义PID控制对象 传入参数为输入和输出的地址及kp ki kd 这里ko为比例
|
// 定义PID控制对象 传入参数为输入和输出的地址及kp ki kd 这里ko为比例
|
||||||
static PID pid1(&input[0], &feedback[0], float(Data_holder::get()->parameter.params.kp)/Data_holder::get()->parameter.params.ko,
|
static PID pid1(&input[0], &feedback[0], float(DataHolder::get()->parameter.params.kp)/DataHolder::get()->parameter.params.ko,
|
||||||
float(Data_holder::get()->parameter.params.ki)/Data_holder::get()->parameter.params.ko,
|
float(DataHolder::get()->parameter.params.ki)/DataHolder::get()->parameter.params.ko,
|
||||||
float(Data_holder::get()->parameter.params.kd)/Data_holder::get()->parameter.params.ko , MAX_PWM_VALUE);
|
float(DataHolder::get()->parameter.params.kd)/DataHolder::get()->parameter.params.ko , MAX_PWM_VALUE);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if MOTOR_COUNT>1 //电机2
|
#if MOTOR_COUNT>1 //电机2
|
||||||
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
|
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
|
||||||
static CommonMotorController motor2(MOTOR_2, MAX_PWM_VALUE, (Data_holder::get()->parameter.params.motor_nonexchange_flag & MOTOR_ENCODER_2_FLAG)==0); // MOTOR1_REVERS标识电机方向反向,等同于电机线交换
|
static CommonMotorController motor2(MOTOR_2, MAX_PWM_VALUE, (DataHolder::get()->parameter.params.motor_nonexchange_flag & MOTOR_ENCODER_2_FLAG)==0); // MOTOR1_REVERS标识电机方向反向,等同于电机线交换
|
||||||
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
|
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
|
||||||
static AFSMotorController motor2(MOTOR_2_PORT_NUM, MAX_PWM_VALUE);
|
static AFSMotorController motor2(MOTOR_2_PORT_NUM, MAX_PWM_VALUE);
|
||||||
#endif
|
#endif
|
||||||
static EncoderImp encoder2(MOTOR_2, (Data_holder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_2_FLAG)==0);
|
static EncoderImp encoder2(MOTOR_2, (DataHolder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_2_FLAG)==0);
|
||||||
static PID pid2(&input[1], &feedback[1], float(Data_holder::get()->parameter.params.kp)/Data_holder::get()->parameter.params.ko,
|
static PID pid2(&input[1], &feedback[1], float(DataHolder::get()->parameter.params.kp)/DataHolder::get()->parameter.params.ko,
|
||||||
float(Data_holder::get()->parameter.params.ki)/Data_holder::get()->parameter.params.ko,
|
float(DataHolder::get()->parameter.params.ki)/DataHolder::get()->parameter.params.ko,
|
||||||
float(Data_holder::get()->parameter.params.kd)/Data_holder::get()->parameter.params.ko , MAX_PWM_VALUE);
|
float(DataHolder::get()->parameter.params.kd)/DataHolder::get()->parameter.params.ko , MAX_PWM_VALUE);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if MOTOR_COUNT>2 //电机3
|
#if MOTOR_COUNT>2 //电机3
|
||||||
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
|
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
|
||||||
static CommonMotorController motor3(MOTOR_3, MAX_PWM_VALUE, (Data_holder::get()->parameter.params.motor_nonexchange_flag & MOTOR_ENCODER_3_FLAG)==0); // MOTOR1_REVERS标识电机方向反向,等同于电机线交换
|
static CommonMotorController motor3(MOTOR_3, MAX_PWM_VALUE, (DataHolder::get()->parameter.params.motor_nonexchange_flag & MOTOR_ENCODER_3_FLAG)==0); // MOTOR1_REVERS标识电机方向反向,等同于电机线交换
|
||||||
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
|
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
|
||||||
static AFSMotorController motor3(MOTOR_3);
|
static AFSMotorController motor3(MOTOR_3);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
static EncoderImp encoder3(MOTOR_3, (Data_holder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_3_FLAG)==0);
|
static EncoderImp encoder3(MOTOR_3, (DataHolder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_3_FLAG)==0);
|
||||||
static PID pid3(&input[2], &feedback[2], float(Data_holder::get()->parameter.params.kp)/Data_holder::get()->parameter.params.ko,
|
static PID pid3(&input[2], &feedback[2], float(DataHolder::get()->parameter.params.kp)/DataHolder::get()->parameter.params.ko,
|
||||||
float(Data_holder::get()->parameter.params.ki)/Data_holder::get()->parameter.params.ko,
|
float(DataHolder::get()->parameter.params.ki)/DataHolder::get()->parameter.params.ko,
|
||||||
float(Data_holder::get()->parameter.params.kd)/Data_holder::get()->parameter.params.ko , MAX_PWM_VALUE);
|
float(DataHolder::get()->parameter.params.kd)/DataHolder::get()->parameter.params.ko , MAX_PWM_VALUE);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// 分别把地址保存在统一的数组内， 方便处理
|
// 分别把地址保存在统一的数组内， 方便处理
|
||||||
|
|
@ -203,12 +201,12 @@ void Robot::init_motor()
|
||||||
|
|
||||||
// 根据定义的模型参数ROBOT_MODEL， 创建模型对象及保存地址
|
// 根据定义的模型参数ROBOT_MODEL， 创建模型对象及保存地址
|
||||||
#if ROBOT_MODEL == MODEL_TYPE_2WD_DIFF //2轮差分
|
#if ROBOT_MODEL == MODEL_TYPE_2WD_DIFF //2轮差分
|
||||||
static Differential diff(Data_holder::get()->parameter.params.wheel_diameter*0.0005, Data_holder::get()->parameter.params.wheel_track*0.0005);
|
static Differential diff(DataHolder::get()->parameter.params.wheel_diameter*0.0005, DataHolder::get()->parameter.params.wheel_track*0.0005);
|
||||||
model = &diff;
|
model = &diff;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if ROBOT_MODEL == MODEL_TYPE_3WD_OMNI //三轮全向
|
#if ROBOT_MODEL == MODEL_TYPE_3WD_OMNI //三轮全向
|
||||||
static Omni3 omni3(Data_holder::get()->parameter.params.wheel_diameter*0.0005, Data_holder::get()->parameter.params.wheel_track*0.0005);
|
static Omni3 omni3(DataHolder::get()->parameter.params.wheel_diameter*0.0005, DataHolder::get()->parameter.params.wheel_track*0.0005);
|
||||||
model = &omni3;
|
model = &omni3;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
@ -228,7 +226,7 @@ void Robot::init_motor()
|
||||||
last_velocity_command_time = 0;
|
last_velocity_command_time = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Robot::init_trans()
|
void Robot::InitTrans()
|
||||||
{
|
{
|
||||||
//定义一个串口通讯对象 ！！！注意这里USART_transport继承实现Transport接口，
|
//定义一个串口通讯对象 ！！！注意这里USART_transport继承实现Transport接口，
|
||||||
// 实现了一个通讯对象，这么做的原因是，如果我换为其他通讯方式，例如USB，只需要创建一个USB_Transport继承实现实现Transport接口就可以实现
|
// 实现了一个通讯对象，这么做的原因是，如果我换为其他通讯方式，例如USB，只需要创建一个USB_Transport继承实现实现Transport接口就可以实现
|
||||||
|
|
@ -254,7 +252,7 @@ void Robot::init_trans()
|
||||||
frame->register_notify(ID_SET_MOTOR_PWM, this);
|
frame->register_notify(ID_SET_MOTOR_PWM, this);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Robot::check_command()
|
void Robot::CheckCommand()
|
||||||
{
|
{
|
||||||
static unsigned long lastMillis = 0;
|
static unsigned long lastMillis = 0;
|
||||||
if (Board::get()->getTickCount() - lastMillis >= 50){
|
if (Board::get()->getTickCount() - lastMillis >= 50){
|
||||||
|
|
@ -263,7 +261,7 @@ void Robot::check_command()
|
||||||
Board::get()->setDOState(_RUN_LED, 2);
|
Board::get()->setDOState(_RUN_LED, 2);
|
||||||
}
|
}
|
||||||
|
|
||||||
unsigned char ch = 0;
|
uint8_t ch = 0;
|
||||||
|
|
||||||
// 从通讯设备(trans)读取数据交给协议处理模块(frame)处理
|
// 从通讯设备(trans)读取数据交给协议处理模块(frame)处理
|
||||||
if (trans->read(ch)) {
|
if (trans->read(ch)) {
|
||||||
|
|
@ -280,28 +278,28 @@ void Robot::update(const MESSAGE_ID id, void* data)
|
||||||
switch (id) {
|
switch (id) {
|
||||||
case ID_SET_ROBOT_PARAMTER: //设置参数的回调， 这里会更新pid的参数， model的参数， 最终保存到flash
|
case ID_SET_ROBOT_PARAMTER: //设置参数的回调， 这里会更新pid的参数， model的参数， 最终保存到flash
|
||||||
|
|
||||||
pb_printf("RobotParameters: %d %d %d %d %d %d %d %d %d %d %d %d %d",
|
log("RobotParameters: %d %d %d %d %d %d %d %d %d %d %d %d %d",
|
||||||
Data_holder::get()->parameter.params.wheel_diameter, Data_holder::get()->parameter.params.wheel_track, Data_holder::get()->parameter.params.encoder_resolution, Data_holder::get()->parameter.params.motor_ratio,
|
DataHolder::get()->parameter.params.wheel_diameter, DataHolder::get()->parameter.params.wheel_track, DataHolder::get()->parameter.params.encoder_resolution, DataHolder::get()->parameter.params.motor_ratio,
|
||||||
Data_holder::get()->parameter.params.do_pid_interval, Data_holder::get()->parameter.params.kp, Data_holder::get()->parameter.params.ki, Data_holder::get()->parameter.params.kd, Data_holder::get()->parameter.params.ko,
|
DataHolder::get()->parameter.params.do_pid_interval, DataHolder::get()->parameter.params.kp, DataHolder::get()->parameter.params.ki, DataHolder::get()->parameter.params.kd, DataHolder::get()->parameter.params.ko,
|
||||||
Data_holder::get()->parameter.params.cmd_last_time, Data_holder::get()->parameter.params.max_v_liner_x, Data_holder::get()->parameter.params.max_v_liner_y, Data_holder::get()->parameter.params.max_v_angular_z);
|
DataHolder::get()->parameter.params.cmd_last_time, DataHolder::get()->parameter.params.max_v_liner_x, DataHolder::get()->parameter.params.max_v_liner_y, DataHolder::get()->parameter.params.max_v_angular_z);
|
||||||
// 更新pid参数， 这样可以保证设置参数实时生效
|
// 更新pid参数， 这样可以保证设置参数实时生效
|
||||||
for (int i=0;i<MOTOR_COUNT;i++) {
|
for (int i=0;i<MOTOR_COUNT;i++) {
|
||||||
pid[i]->update(float(Data_holder::get()->parameter.params.kp)/Data_holder::get()->parameter.params.ko,
|
pid[i]->update(float(DataHolder::get()->parameter.params.kp)/DataHolder::get()->parameter.params.ko,
|
||||||
float(Data_holder::get()->parameter.params.ki)/Data_holder::get()->parameter.params.ko,
|
float(DataHolder::get()->parameter.params.ki)/DataHolder::get()->parameter.params.ko,
|
||||||
float(Data_holder::get()->parameter.params.kd)/Data_holder::get()->parameter.params.ko , MAX_PWM_VALUE);
|
float(DataHolder::get()->parameter.params.kd)/DataHolder::get()->parameter.params.ko , MAX_PWM_VALUE);
|
||||||
}
|
}
|
||||||
|
|
||||||
// 更新模型参数， 这样可以保证设置参数实时生效
|
// 更新模型参数， 这样可以保证设置参数实时生效
|
||||||
model->set(Data_holder::get()->parameter.params.wheel_diameter*0.0005, Data_holder::get()->parameter.params.wheel_track*0.0005);
|
model->set(DataHolder::get()->parameter.params.wheel_diameter*0.0005, DataHolder::get()->parameter.params.wheel_track*0.0005);
|
||||||
|
|
||||||
// 保存参数到flash
|
// 保存参数到flash
|
||||||
Data_holder::get()->save_parameter();
|
DataHolder::get()->save_parameter();
|
||||||
break;
|
break;
|
||||||
case ID_CLEAR_ODOM: // 清除里程计信息
|
case ID_CLEAR_ODOM: // 清除里程计信息
|
||||||
clear_odom();
|
clear_odom();
|
||||||
break;
|
break;
|
||||||
case ID_SET_VELOCITY: // 设置机器人的速度
|
case ID_SET_VELOCITY: // 设置机器人的速度
|
||||||
update_velocity(); //更新机器人的期望速度
|
updateVelocity(); //更新机器人的期望速度
|
||||||
break;
|
break;
|
||||||
case ID_GET_ENCODER_COUNT:
|
case ID_GET_ENCODER_COUNT:
|
||||||
update_encoder_count(); // 更新encoder count
|
update_encoder_count(); // 更新encoder count
|
||||||
|
|
@ -322,30 +320,32 @@ void Robot::clear_odom()
|
||||||
}
|
}
|
||||||
|
|
||||||
memset(&odom, 0, sizeof(odom));
|
memset(&odom, 0, sizeof(odom));
|
||||||
memset(&Data_holder::get()->odom, 0, sizeof(Data_holder::get()->odom));
|
memset(&DataHolder::get()->odom, 0, sizeof(DataHolder::get()->odom));
|
||||||
}
|
}
|
||||||
|
|
||||||
#define __PI 3.1415926535897932384626433832795
|
#define __PI 3.1415926535897932384626433832795
|
||||||
|
|
||||||
//根据下发的速度更新期望速度， 并转为pid时间间隔内的期望编码器的变化值
|
// 根据下发的速度更新期望速度， 并转为pid时间间隔内的期望编码器的变化值
|
||||||
void Robot::update_velocity()
|
void Robot::updateVelocity()
|
||||||
{
|
{
|
||||||
// 下发速度检测 保证限制在设置的最大最小值内
|
// 下发速度检测 保证限制在设置的最大最小值内
|
||||||
short vx = min(max(Data_holder::get()->velocity.v_liner_x, -(short(Data_holder::get()->parameter.params.max_v_liner_x))), short(Data_holder::get()->parameter.params.max_v_liner_x));
|
short vx = min(max(DataHolder::get()->velocity.v_liner_x, -(short(DataHolder::get()->parameter.params.max_v_liner_x))), short(DataHolder::get()->parameter.params.max_v_liner_x));
|
||||||
short vy = min(max(Data_holder::get()->velocity.v_liner_y, -(short(Data_holder::get()->parameter.params.max_v_liner_y))), short(Data_holder::get()->parameter.params.max_v_liner_y));
|
short vy = min(max(DataHolder::get()->velocity.v_liner_y, -(short(DataHolder::get()->parameter.params.max_v_liner_y))), short(DataHolder::get()->parameter.params.max_v_liner_y));
|
||||||
short vz = min(max(Data_holder::get()->velocity.v_angular_z, -(short(Data_holder::get()->parameter.params.max_v_angular_z))), short(Data_holder::get()->parameter.params.max_v_angular_z));
|
short vz = min(max(DataHolder::get()->velocity.v_angular_z, -(short(DataHolder::get()->parameter.params.max_v_angular_z))), short(DataHolder::get()->parameter.params.max_v_angular_z));
|
||||||
|
|
||||||
float vel[3]={vx/100.0, vy/100.0, vz/100.0}; // 保存速度转换单位 传入的速度值为cm/s 0.01rad/s 转为m/s 和rad/s
|
// 保存速度转换单位,传入的速度值为cm/s 0.01rad/s 转为m/s 和rad/s
|
||||||
|
float vel[3]={vx/100.0, vy/100.0, vz/100.0};
|
||||||
float motor_speed[MOTOR_COUNT]={0};
|
float motor_speed[MOTOR_COUNT]={0};
|
||||||
model->motion_solver(vel, motor_speed); // 期望的机器人的速度转换为各个电机的速度(通过设置的机器人模型接口)
|
|
||||||
|
model->motionSolver(vel, motor_speed); // 期望的机器人的速度转换为各个电机的速度(通过设置的机器人模型接口)
|
||||||
|
|
||||||
// 把得到的期望电机速度 (m/s)转换为期望pid时间间隔内编码器反馈的输入
|
// 把得到的期望电机速度 (m/s)转换为期望pid时间间隔内编码器反馈的输入
|
||||||
// 该值即为PID的输入
|
// 该值即为PID的输入
|
||||||
for(int i=0;i<MOTOR_COUNT;i++) {
|
for(int i = 0;i < MOTOR_COUNT; i++) {
|
||||||
input[i] = motor_speed[i]*short(Data_holder::get()->parameter.params.encoder_resolution)*short(Data_holder::get()->parameter.params.motor_ratio)/(2*__PI)*short(Data_holder::get()->parameter.params.do_pid_interval)*0.001;
|
input[i] = motor_speed[i]*short(DataHolder::get()->parameter.params.encoder_resolution)*short(DataHolder::get()->parameter.params.motor_ratio)/(2*__PI)*short(DataHolder::get()->parameter.params.do_pid_interval)*0.001;
|
||||||
}
|
}
|
||||||
|
|
||||||
//pb_printf("vx=%d %d motor_speed=%ld %ld", vx, vz, long(motor_speed[0]*1000), long(motor_speed[1]*1000));
|
//log("vx=%d %d motor_speed=%ld %ld", vx, vz, long(motor_speed[0]*1000), long(motor_speed[1]*1000));
|
||||||
|
|
||||||
//保存时间戳
|
//保存时间戳
|
||||||
last_velocity_command_time = Board::get()->getTickCount();
|
last_velocity_command_time = Board::get()->getTickCount();
|
||||||
|
|
@ -357,7 +357,7 @@ void Robot::update_velocity()
|
||||||
void Robot::update_encoder_count()
|
void Robot::update_encoder_count()
|
||||||
{
|
{
|
||||||
for (int i=0; i<MOTOR_COUNT; i++) {
|
for (int i=0; i<MOTOR_COUNT; i++) {
|
||||||
Data_holder::get()->encoder_count[i] = encoder[i]->get_total_count();//输出累计编码器值 用于调试
|
DataHolder::get()->encoder_count[i] = encoder[i]->get_total_count();//输出累计编码器值 用于调试
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -367,13 +367,13 @@ void Robot::update_pwm_value()
|
||||||
}
|
}
|
||||||
|
|
||||||
// pid运算， 控制电机
|
// pid运算， 控制电机
|
||||||
void Robot::do_kinmatics()
|
void Robot::DoKinmatics()
|
||||||
{
|
{
|
||||||
if (do_set_pwm_flag) {
|
if (do_set_pwm_flag) {
|
||||||
do_set_pwm_flag = false;
|
do_set_pwm_flag = false;
|
||||||
|
|
||||||
for (int i=0; i<MOTOR_COUNT; i++) {
|
for (int i=0; i<MOTOR_COUNT; i++) {
|
||||||
motor[i]->control(Data_holder::get()->pwm.value[i]);
|
motor[i]->control(DataHolder::get()->pwm.value[i]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -387,7 +387,7 @@ void Robot::do_kinmatics()
|
||||||
|
|
||||||
static unsigned long last_millis=0;
|
static unsigned long last_millis=0;
|
||||||
// 判断时间间隔做pid运算
|
// 判断时间间隔做pid运算
|
||||||
if (Board::get()->getTickCount()-last_millis>=Data_holder::get()->parameter.params.do_pid_interval) {
|
if (Board::get()->getTickCount()-last_millis>=DataHolder::get()->parameter.params.do_pid_interval) {
|
||||||
last_millis = Board::get()->getTickCount();
|
last_millis = Board::get()->getTickCount();
|
||||||
|
|
||||||
//得到编码器的反馈差值， 即在do_pid_interval这段间隔时间内编码器的差值， 该值作为pid的反馈
|
//得到编码器的反馈差值， 即在do_pid_interval这段间隔时间内编码器的差值， 该值作为pid的反馈
|
||||||
|
|
@ -396,15 +396,15 @@ void Robot::do_kinmatics()
|
||||||
}
|
}
|
||||||
#if PID_DEBUG_OUTPUT
|
#if PID_DEBUG_OUTPUT
|
||||||
#if MOTOR_COUNT==2
|
#if MOTOR_COUNT==2
|
||||||
pb_printf("input=%ld %ld feedback=%ld %ld", long(input[0]*1000), long(input[1]*1000),
|
log("input=%ld %ld feedback=%ld %ld", long(input[0]*1000), long(input[1]*1000),
|
||||||
long(feedback[0]), long(feedback[1]));
|
long(feedback[0]), long(feedback[1]));
|
||||||
#endif
|
#endif
|
||||||
#if MOTOR_COUNT==3
|
#if MOTOR_COUNT==3
|
||||||
pb_printf("input=%ld %ld %ld feedback=%ld %ld %ld", long(input[0]*1000), long(input[1]*1000), long(input[2]*1000),
|
log("input=%ld %ld %ld feedback=%ld %ld %ld", long(input[0]*1000), long(input[1]*1000), long(input[2]*1000),
|
||||||
long(feedback[0]), long(feedback[1]), long(feedback[2]));
|
long(feedback[0]), long(feedback[1]), long(feedback[2]));
|
||||||
#endif
|
#endif
|
||||||
#if MOTOR_COUNT==4
|
#if MOTOR_COUNT==4
|
||||||
pb_printf("input=%ld %ld %ld %ld feedback=%ld %ld %ld %ld", long(input[0]*1000), long(input[1]*1000), long(input[2]*1000), long(input[3]*1000),
|
log("input=%ld %ld %ld %ld feedback=%ld %ld %ld %ld", long(input[0]*1000), long(input[1]*1000), long(input[2]*1000), long(input[3]*1000),
|
||||||
long(feedback[0]), long(feedback[1]), long(feedback[2]), long(feedback[3]));
|
long(feedback[0]), long(feedback[1]), long(feedback[2]), long(feedback[3]));
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
|
@ -423,33 +423,33 @@ void Robot::do_kinmatics()
|
||||||
} else {
|
} else {
|
||||||
// 没有需要停止则做pid运算，得到out值，该值作为电机模块控制器输入
|
// 没有需要停止则做pid运算，得到out值，该值作为电机模块控制器输入
|
||||||
for(int i=0;i<MOTOR_COUNT;i++) {
|
for(int i=0;i<MOTOR_COUNT;i++) {
|
||||||
output[i] = pid[i]->compute(Data_holder::get()->parameter.params.do_pid_interval*0.001);
|
output[i] = pid[i]->compute(DataHolder::get()->parameter.params.do_pid_interval*0.001);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//当次计算完成 重置变量
|
//当次计算完成 重置变量
|
||||||
for (int i=0;i<MOTOR_COUNT;i++) {
|
for (int i=0;i<MOTOR_COUNT;i++) {
|
||||||
Data_holder::get()->pid_data.input[i] = int(input[i]);
|
DataHolder::get()->pid_data.input[i] = int(input[i]);
|
||||||
Data_holder::get()->pid_data.output[i] = int(feedback[i]);
|
DataHolder::get()->pid_data.output[i] = int(feedback[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
#if PID_DEBUG_OUTPUT
|
#if PID_DEBUG_OUTPUT
|
||||||
#if MOTOR_COUNT==2
|
#if MOTOR_COUNT==2
|
||||||
pb_printf("output=%ld %ld", output[0], output[1]);
|
log("output=%ld %ld", output[0], output[1]);
|
||||||
#endif
|
#endif
|
||||||
#if MOTOR_COUNT==3
|
#if MOTOR_COUNT==3
|
||||||
pb_printf("output=%ld %ld %ld", output[0], output[1], output[2]);
|
log("output=%ld %ld %ld", output[0], output[1], output[2]);
|
||||||
#endif
|
#endif
|
||||||
#if MOTOR_COUNT==4
|
#if MOTOR_COUNT==4
|
||||||
pb_printf("output=%ld %ld %ld %ld", output[0], output[1], output[2], output[3]);
|
log("output=%ld %ld %ld %ld", output[0], output[1], output[2], output[3]);
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
long elapsed_ms = Board::get()->getTickCount()-last_velocity_command_time;
|
long elapsed_ms = Board::get()->getTickCount()-last_velocity_command_time;
|
||||||
//判断上次下发的时间戳，如果超时，把各个电机期望的输入置零，pid下次会根据改期望计算pid输出转速，慢慢停止点击
|
//判断上次下发的时间戳，如果超时，把各个电机期望的输入置零，pid下次会根据改期望计算pid输出转速，慢慢停止点击
|
||||||
if (elapsed_ms > Data_holder::get()->parameter.params.cmd_last_time) {
|
if (elapsed_ms > DataHolder::get()->parameter.params.cmd_last_time) {
|
||||||
memset(input, 0, sizeof(input));
|
memset(input, 0, sizeof(input));
|
||||||
if (elapsed_ms > Data_holder::get()->parameter.params.cmd_last_time*2) {
|
if (elapsed_ms > DataHolder::get()->parameter.params.cmd_last_time*2) {
|
||||||
memset(output, 0, sizeof(output));
|
memset(output, 0, sizeof(output));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -462,7 +462,7 @@ void Robot::do_kinmatics()
|
||||||
}
|
}
|
||||||
|
|
||||||
//计算里程计
|
//计算里程计
|
||||||
void Robot::calc_odom()
|
void Robot::CalcOdom()
|
||||||
{
|
{
|
||||||
static unsigned long last_millis=0;
|
static unsigned long last_millis=0;
|
||||||
//根据实际间隔计算轮子里程
|
//根据实际间隔计算轮子里程
|
||||||
|
|
@ -475,48 +475,44 @@ void Robot::calc_odom()
|
||||||
}
|
}
|
||||||
|
|
||||||
#if MOTOR_COUNT==2
|
#if MOTOR_COUNT==2
|
||||||
pb_printf("total_count=[%ld %ld]", total_count[0], total_count[1]);
|
log("total_count=[%ld %ld]", total_count[0], total_count[1]);
|
||||||
#endif
|
#endif
|
||||||
#if MOTOR_COUNT==3
|
#if MOTOR_COUNT==3
|
||||||
pb_printf("total_count=[%ld %ld %ld]", total_count[0], total_count[1], total_count[2]);
|
log("total_count=[%ld %ld %ld]", total_count[0], total_count[1], total_count[2]);
|
||||||
#endif
|
#endif
|
||||||
#if MOTOR_COUNT==4
|
#if MOTOR_COUNT==4
|
||||||
pb_printf("total_count=[%ld %ld %ld %ld]", total_count[0], total_count[1], total_count[2], total_count[3]);
|
log("total_count=[%ld %ld %ld %ld]", total_count[0], total_count[1], total_count[2], total_count[3]);
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
float dis[MOTOR_COUNT] = {0};
|
float dis[MOTOR_COUNT] = {0};
|
||||||
for (int i=0;i<MOTOR_COUNT;i++) {
|
for (int i=0;i<MOTOR_COUNT;i++) {
|
||||||
//根据CALC_ODOM_INTERVAL的间隔内的各个电机的编码器变化值，转换各个电机实际的里程
|
//根据CALC_ODOM_INTERVAL的间隔内的各个电机的编码器变化值，转换各个电机实际的里程
|
||||||
dis[i] = encoder[i]->get_increment_count_for_odom()*__PI*Data_holder::get()->parameter.params.wheel_diameter*0.001/Data_holder::get()->parameter.params.encoder_resolution/Data_holder::get()->parameter.params.motor_ratio;
|
dis[i] = encoder[i]->get_increment_count_for_odom()*__PI*DataHolder::get()->parameter.params.wheel_diameter*0.001/DataHolder::get()->parameter.params.encoder_resolution/DataHolder::get()->parameter.params.motor_ratio;
|
||||||
#if ODOM_DEBUG_OUTPUT
|
#if ODOM_DEBUG_OUTPUT
|
||||||
pb_printf(" %ld ", long(dis[i]*1000000));
|
log(" %ld ", long(dis[i]*1000000));
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
// long total_count[MOTOR_COUNT]={0};
|
|
||||||
// for (int i=0;i<MOTOR_COUNT;i++) {
|
|
||||||
// total_count[i] = encoder[i]->get_total_count(); //输出累计编码器值 用于调试
|
|
||||||
// }
|
|
||||||
// 把计算得到的各个电机的里程转为为机器人里程(通过机器人运动模型对象)
|
// 把计算得到的各个电机的里程转为为机器人里程(通过机器人运动模型对象)
|
||||||
model->get_odom(&odom, dis, CALC_ODOM_INTERVAL);
|
model->get_odom(&odom, dis, CALC_ODOM_INTERVAL);
|
||||||
|
|
||||||
#if ODOM_DEBUG_OUTPUT
|
#if ODOM_DEBUG_OUTPUT
|
||||||
// 输出机器人的里程
|
// 输出机器人的里程
|
||||||
pb_printf(" x=%ld y=%ld yaw=%ld", long(odom.x*1000), long(odom.y*1000), long(odom.z*1000));//mm
|
log(" x=%ld y=%ld yaw=%ld", long(odom.x*1000), long(odom.y*1000), long(odom.z*1000));//mm
|
||||||
pb_printf("");
|
log("");
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// 转换单位保存到Data_holder
|
// 转换单位保存到DataHolder
|
||||||
Data_holder::get()->odom.v_liner_x = odom.vel_x*100; //转为cm/s
|
DataHolder::get()->odom.v_liner_x = odom.vel_x*100; //转为cm/s
|
||||||
Data_holder::get()->odom.v_liner_y = odom.vel_y*100;
|
DataHolder::get()->odom.v_liner_y = odom.vel_y*100;
|
||||||
Data_holder::get()->odom.v_angular_z = odom.vel_z*100;
|
DataHolder::get()->odom.v_angular_z = odom.vel_z*100;
|
||||||
Data_holder::get()->odom.x = odom.x*100; //转为cm
|
DataHolder::get()->odom.x = odom.x*100; //转为cm
|
||||||
Data_holder::get()->odom.y = odom.y*100;
|
DataHolder::get()->odom.y = odom.y*100;
|
||||||
Data_holder::get()->odom.yaw = long(odom.z*100)%628;//转为0.01rad 628为2pi*100
|
DataHolder::get()->odom.yaw = long(odom.z*100)%628;//转为0.01rad 628为2pi*100
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//获取imu数据
|
//获取imu数据
|
||||||
void Robot::get_imu_data()
|
void Robot::GetImuData()
|
||||||
{
|
{
|
||||||
#if IMU_ENABLE
|
#if IMU_ENABLE
|
||||||
if (imu == NULL) {
|
if (imu == NULL) {
|
||||||
|
|
@ -525,15 +521,15 @@ void Robot::get_imu_data()
|
||||||
|
|
||||||
static unsigned long last_millis=0;
|
static unsigned long last_millis=0;
|
||||||
//按照设置的时间间隔获取imu数据
|
//按照设置的时间间隔获取imu数据
|
||||||
if (Board::get()->get_tick_count()-last_millis>=CALC_IMU_INTERVAL) {
|
if (Board::get()->getTickCount()-last_millis>=CALC_IMU_INTERVAL) {
|
||||||
last_millis = Board::get()->get_tick_count();
|
last_millis = Board::get()->getTickCount();
|
||||||
imu->get_data(Data_holder::get()->imu_data);
|
imu->get_data(DataHolder::get()->imu_data);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
//检测joystick
|
//检测joystick
|
||||||
void Robot::check_joystick()
|
void Robot::CheckJoystick()
|
||||||
{
|
{
|
||||||
#if JOYSTICK_ENABLE
|
#if JOYSTICK_ENABLE
|
||||||
if (joystick == NULL) {
|
if (joystick == NULL) {
|
||||||
|
|
@ -542,13 +538,13 @@ void Robot::check_joystick()
|
||||||
|
|
||||||
static unsigned long last_millis=0;
|
static unsigned long last_millis=0;
|
||||||
short liner_x=0, liner_y=0, angular_z=0;
|
short liner_x=0, liner_y=0, angular_z=0;
|
||||||
if (Board::get()->get_tick_count()-last_millis>=CHECK_JOYSTICK_INTERVAL){
|
if (Board::get()->getTickCount()-last_millis>=CHECK_JOYSTICK_INTERVAL){
|
||||||
last_millis = Board::get()->get_tick_count();
|
last_millis = Board::get()->getTickCount();
|
||||||
// 按照设置的间隔间隔 更新期望速度
|
// 按照设置的间隔间隔 更新期望速度
|
||||||
if (joystick->check(liner_x, liner_y, angular_z)) {
|
if (joystick->check(liner_x, liner_y, angular_z)) {
|
||||||
Data_holder::get()->velocity.v_liner_x = liner_x;
|
DataHolder::get()->velocity.v_liner_x = liner_x;
|
||||||
Data_holder::get()->velocity.v_liner_y = liner_y;
|
DataHolder::get()->velocity.v_liner_y = liner_y;
|
||||||
Data_holder::get()->velocity.v_angular_z = angular_z;
|
DataHolder::get()->velocity.v_angular_z = angular_z;
|
||||||
update_velocity();
|
update_velocity();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,7 @@
|
||||||
#ifndef PIBOT_ROBOT_H_
|
#ifndef PIBOT_ROBOT_H_
|
||||||
#define PIBOT_ROBOT_H_
|
#define PIBOT_ROBOT_H_
|
||||||
|
|
||||||
|
#include "stdint.h"
|
||||||
#include "dataframe.h"
|
#include "dataframe.h"
|
||||||
|
|
||||||
#if ROBOT_MODEL == MODEL_TYPE_2WD_DIFF
|
#if ROBOT_MODEL == MODEL_TYPE_2WD_DIFF
|
||||||
|
|
@ -30,22 +31,22 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
// 初始化
|
// 初始化
|
||||||
void init();
|
void Init();
|
||||||
|
|
||||||
// 检测串口数据命令
|
// 检测串口数据命令
|
||||||
void check_command();
|
void CheckCommand();
|
||||||
|
|
||||||
// 运动解算
|
// 运动解算
|
||||||
void do_kinmatics();
|
void DoKinmatics();
|
||||||
|
|
||||||
// 计算里程计
|
// 计算里程计
|
||||||
void calc_odom();
|
void CalcOdom();
|
||||||
|
|
||||||
//获取imu数据
|
//获取imu数据
|
||||||
void get_imu_data();
|
void GetImuData();
|
||||||
|
|
||||||
// 检测joystick的按键
|
// 检测joystick的按键
|
||||||
void check_joystick();
|
void CheckJoystick();
|
||||||
|
|
||||||
// 实现Notify接口处理 绑定的消息
|
// 实现Notify接口处理 绑定的消息
|
||||||
virtual void update(const MESSAGE_ID id, void* data);
|
virtual void update(const MESSAGE_ID id, void* data);
|
||||||
|
|
@ -53,15 +54,15 @@ private:
|
||||||
Robot();
|
Robot();
|
||||||
//初始化
|
//初始化
|
||||||
void init_imu();
|
void init_imu();
|
||||||
void init_joystick();
|
void InitJoystick();
|
||||||
void init_motor();
|
void InitMotor();
|
||||||
void init_trans();
|
void InitTrans();
|
||||||
private:
|
private:
|
||||||
// 清除里程计
|
// 清除里程计
|
||||||
void clear_odom();
|
void clear_odom();
|
||||||
|
|
||||||
// 更新当前机器人的速度
|
// 更新当前机器人的速度
|
||||||
void update_velocity();
|
void updateVelocity();
|
||||||
|
|
||||||
// 更新编码器值
|
// 更新编码器值
|
||||||
void update_encoder_count();
|
void update_encoder_count();
|
||||||
|
|
|
||||||
|
|
@ -506,18 +506,6 @@
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\Lib\DataHolder\data_holder.cpp</PathWithFileName>
|
|
||||||
<FilenameWithoutPath>data_holder.cpp</FilenameWithoutPath>
|
|
||||||
<RteFlg>0</RteFlg>
|
|
||||||
<bShared>0</bShared>
|
|
||||||
</File>
|
|
||||||
<File>
|
|
||||||
<GroupNumber>5</GroupNumber>
|
|
||||||
<FileNumber>24</FileNumber>
|
|
||||||
<FileType>8</FileType>
|
|
||||||
<tvExp>0</tvExp>
|
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
|
||||||
<bDave2>0</bDave2>
|
|
||||||
<PathWithFileName>.\Lib\Board\board_stm32.cpp</PathWithFileName>
|
<PathWithFileName>.\Lib\Board\board_stm32.cpp</PathWithFileName>
|
||||||
<FilenameWithoutPath>board_stm32.cpp</FilenameWithoutPath>
|
<FilenameWithoutPath>board_stm32.cpp</FilenameWithoutPath>
|
||||||
<RteFlg>0</RteFlg>
|
<RteFlg>0</RteFlg>
|
||||||
|
|
@ -525,7 +513,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>5</GroupNumber>
|
<GroupNumber>5</GroupNumber>
|
||||||
<FileNumber>25</FileNumber>
|
<FileNumber>24</FileNumber>
|
||||||
<FileType>8</FileType>
|
<FileType>8</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
|
|
@ -537,7 +525,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>5</GroupNumber>
|
<GroupNumber>5</GroupNumber>
|
||||||
<FileNumber>26</FileNumber>
|
<FileNumber>25</FileNumber>
|
||||||
<FileType>8</FileType>
|
<FileType>8</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
|
|
@ -549,7 +537,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>5</GroupNumber>
|
<GroupNumber>5</GroupNumber>
|
||||||
<FileNumber>27</FileNumber>
|
<FileNumber>26</FileNumber>
|
||||||
<FileType>8</FileType>
|
<FileType>8</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
|
|
@ -561,7 +549,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>5</GroupNumber>
|
<GroupNumber>5</GroupNumber>
|
||||||
<FileNumber>28</FileNumber>
|
<FileNumber>27</FileNumber>
|
||||||
<FileType>8</FileType>
|
<FileType>8</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
|
|
@ -573,7 +561,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>5</GroupNumber>
|
<GroupNumber>5</GroupNumber>
|
||||||
<FileNumber>29</FileNumber>
|
<FileNumber>28</FileNumber>
|
||||||
<FileType>8</FileType>
|
<FileType>8</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
|
|
@ -585,7 +573,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>5</GroupNumber>
|
<GroupNumber>5</GroupNumber>
|
||||||
<FileNumber>30</FileNumber>
|
<FileNumber>29</FileNumber>
|
||||||
<FileType>8</FileType>
|
<FileType>8</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
|
|
@ -597,7 +585,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>5</GroupNumber>
|
<GroupNumber>5</GroupNumber>
|
||||||
<FileNumber>31</FileNumber>
|
<FileNumber>30</FileNumber>
|
||||||
<FileType>8</FileType>
|
<FileType>8</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
|
|
@ -609,7 +597,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>5</GroupNumber>
|
<GroupNumber>5</GroupNumber>
|
||||||
<FileNumber>32</FileNumber>
|
<FileNumber>31</FileNumber>
|
||||||
<FileType>8</FileType>
|
<FileType>8</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
|
|
@ -621,7 +609,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>5</GroupNumber>
|
<GroupNumber>5</GroupNumber>
|
||||||
<FileNumber>33</FileNumber>
|
<FileNumber>32</FileNumber>
|
||||||
<FileType>8</FileType>
|
<FileType>8</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
|
|
@ -631,6 +619,18 @@
|
||||||
<RteFlg>0</RteFlg>
|
<RteFlg>0</RteFlg>
|
||||||
<bShared>0</bShared>
|
<bShared>0</bShared>
|
||||||
</File>
|
</File>
|
||||||
|
<File>
|
||||||
|
<GroupNumber>5</GroupNumber>
|
||||||
|
<FileNumber>33</FileNumber>
|
||||||
|
<FileType>8</FileType>
|
||||||
|
<tvExp>0</tvExp>
|
||||||
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
|
<bDave2>0</bDave2>
|
||||||
|
<PathWithFileName>.\Lib\DataHolder\data_holder.cpp</PathWithFileName>
|
||||||
|
<FilenameWithoutPath>data_holder.cpp</FilenameWithoutPath>
|
||||||
|
<RteFlg>0</RteFlg>
|
||||||
|
<bShared>0</bShared>
|
||||||
|
</File>
|
||||||
</Group>
|
</Group>
|
||||||
|
|
||||||
<Group>
|
<Group>
|
||||||
|
|
|
||||||
|
|
@ -509,11 +509,6 @@
|
||||||
<FileType>8</FileType>
|
<FileType>8</FileType>
|
||||||
<FilePath>.\Lib\Encoder\encoder_implement.cpp</FilePath>
|
<FilePath>.\Lib\Encoder\encoder_implement.cpp</FilePath>
|
||||||
</File>
|
</File>
|
||||||
<File>
|
|
||||||
<FileName>data_holder.cpp</FileName>
|
|
||||||
<FileType>8</FileType>
|
|
||||||
<FilePath>.\Lib\DataHolder\data_holder.cpp</FilePath>
|
|
||||||
</File>
|
|
||||||
<File>
|
<File>
|
||||||
<FileName>board_stm32.cpp</FileName>
|
<FileName>board_stm32.cpp</FileName>
|
||||||
<FileType>8</FileType>
|
<FileType>8</FileType>
|
||||||
|
|
@ -564,6 +559,11 @@
|
||||||
<FileType>8</FileType>
|
<FileType>8</FileType>
|
||||||
<FilePath>.\Lib\IMU\GY87.cpp</FilePath>
|
<FilePath>.\Lib\IMU\GY87.cpp</FilePath>
|
||||||
</File>
|
</File>
|
||||||
|
<File>
|
||||||
|
<FileName>data_holder.cpp</FileName>
|
||||||
|
<FileType>8</FileType>
|
||||||
|
<FilePath>.\Lib\DataHolder\data_holder.cpp</FilePath>
|
||||||
|
</File>
|
||||||
</Files>
|
</Files>
|
||||||
</Group>
|
</Group>
|
||||||
<Group>
|
<Group>
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue