Skip to content

Commit 5315f3c

Browse files
alfranalfran
committed
added support for Star OTTO Board
1 parent 9e77354 commit 5315f3c

File tree

5 files changed

+428
-1
lines changed

5 files changed

+428
-1
lines changed

‎src/STM32F4/Icon

Whitespace-only changes.

‎src/STM32F4/Servo.cpp

Lines changed: 190 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,190 @@
1+
/******************************************************************************
2+
* The MIT License
3+
*
4+
* Copyright (c) 2010, LeafLabs, LLC.
5+
*
6+
* Permission is hereby granted, free of charge, to any person
7+
* obtaining a copy of this software and associated documentation
8+
* files (the "Software"), to deal in the Software without
9+
* restriction, including without limitation the rights to use, copy,
10+
* modify, merge, publish, distribute, sublicense, and/or sell copies
11+
* of the Software, and to permit persons to whom the Software is
12+
* furnished to do so, subject to the following conditions:
13+
*
14+
* The above copyright notice and this permission notice shall be
15+
* included in all copies or substantial portions of the Software.
16+
*
17+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
18+
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
19+
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
20+
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
21+
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
22+
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
23+
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24+
* SOFTWARE.
25+
*****************************************************************************/
26+
27+
#include "ServoTimers.h"
28+
29+
#include "boards.h"
30+
#include "io.h"
31+
#include "pwm.h"
32+
#include "math.h"
33+
34+
// 20 millisecond period config. For a 1-based prescaler,
35+
//
36+
// (prescaler * overflow / CYC_MSEC) msec = 1 timer cycle = 20 msec
37+
// => prescaler * overflow = 20 * CYC_MSEC
38+
//
39+
// This picks the smallest prescaler that allows an overflow < 2^16.
40+
#define MAX_OVERFLOW ((1 << 16) - 1)
41+
#define CYC_MSEC (1000 * CYCLES_PER_MICROSECOND)
42+
#define TAU_MSEC 20
43+
#define TAU_USEC (TAU_MSEC * 1000)
44+
#define TAU_CYC (TAU_MSEC * CYC_MSEC)
45+
#define SERVO_PRESCALER (TAU_CYC / MAX_OVERFLOW + 1)
46+
#define SERVO_OVERFLOW ((uint16)round((double)TAU_CYC / SERVO_PRESCALER))
47+
48+
// Unit conversions
49+
#define US_TO_COMPARE(us) ((uint16)map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW))
50+
#define COMPARE_TO_US(c) ((uint32)map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC))
51+
#define ANGLE_TO_US(a) ((uint16)(map((a), this->minAngle, this->maxAngle, \
52+
this->minPW, this->maxPW)))
53+
#define US_TO_ANGLE(us) ((int16)(map((us), this->minPW, this->maxPW, \
54+
this->minAngle, this->maxAngle)))
55+
56+
Servo::Servo() {
57+
this->resetFields();
58+
}
59+
60+
bool Servo::attach(uint8 pin, uint16 minPW, uint16 maxPW, int16 minAngle, int16 maxAngle)
61+
{
62+
// SerialUSB.begin(115200);
63+
// SerialUSB.println(MAX_OVERFLOW);
64+
65+
66+
timer_dev *tdev = PIN_MAP[pin].timer_device;
67+
68+
analogWriteResolution(16);
69+
70+
int prescaler = 6;
71+
int overflow = 65400;
72+
int minPW_correction = 300;
73+
int maxPW_correction = 300;
74+
75+
pinMode(pin, OUTPUT);
76+
77+
78+
if (tdev == NULL) {
79+
// don't reset any fields or ASSERT(0), to keep driving any
80+
// previously attach()ed servo.
81+
return false;
82+
}
83+
84+
if ( (tdev == TIMER1) || (tdev == TIMER8) || (tdev == TIMER10) || (tdev == TIMER11))
85+
{
86+
prescaler = 54;
87+
overflow = 65400;
88+
minPW_correction = 40;
89+
maxPW_correction = 50;
90+
}
91+
92+
if ( (tdev == TIMER2) || (tdev == TIMER3) || (tdev == TIMER4) || (tdev == TIMER5) )
93+
{
94+
prescaler = 6;
95+
overflow = 64285;
96+
minPW_correction = 370;
97+
maxPW_correction = 350;
98+
}
99+
100+
if ( (tdev == TIMER6) || (tdev == TIMER7) )
101+
{
102+
prescaler = 6;
103+
overflow = 65400;
104+
minPW_correction = 0;
105+
maxPW_correction = 0;
106+
}
107+
108+
if ( (tdev == TIMER9) || (tdev == TIMER12) || (tdev == TIMER13) || (tdev == TIMER14) )
109+
{
110+
prescaler = 6;
111+
overflow = 65400;
112+
minPW_correction = 30;
113+
maxPW_correction = 0;
114+
}
115+
116+
if (this->attached()) {
117+
this->detach();
118+
}
119+
120+
this->pin = pin;
121+
this->minPW = (minPW + minPW_correction);
122+
this->maxPW = (maxPW + maxPW_correction);
123+
this->minAngle = minAngle;
124+
this->maxAngle = maxAngle;
125+
126+
timer_pause(tdev);
127+
timer_set_prescaler(tdev, prescaler); // prescaler is 1-based
128+
timer_set_reload(tdev, overflow);
129+
timer_generate_update(tdev);
130+
timer_resume(tdev);
131+
132+
return true;
133+
}
134+
135+
bool Servo::detach() {
136+
if (!this->attached()) {
137+
return false;
138+
}
139+
140+
timer_dev *tdev = PIN_MAP[this->pin].timer_device;
141+
uint8 tchan = PIN_MAP[this->pin].timer_channel;
142+
timer_set_mode(tdev, tchan, TIMER_DISABLED);
143+
144+
this->resetFields();
145+
146+
return true;
147+
}
148+
149+
void Servo::write(int degrees) {
150+
degrees = constrain(degrees, this->minAngle, this->maxAngle);
151+
this->writeMicroseconds(ANGLE_TO_US(degrees));
152+
}
153+
154+
int Servo::read() const {
155+
int a = US_TO_ANGLE(this->readMicroseconds());
156+
// map() round-trips in a weird way we mostly correct for here;
157+
// the round-trip is still sometimes off-by-one for write(1) and
158+
// write(179).
159+
return a == this->minAngle || a == this->maxAngle ? a : a + 1;
160+
}
161+
162+
void Servo::writeMicroseconds(uint16 pulseWidth) {
163+
if (!this->attached()) {
164+
ASSERT(0);
165+
return;
166+
}
167+
pulseWidth = constrain(pulseWidth, this->minPW, this->maxPW);
168+
analogWrite(this->pin, US_TO_COMPARE(pulseWidth));
169+
}
170+
171+
uint16 Servo::readMicroseconds() const {
172+
if (!this->attached()) {
173+
ASSERT(0);
174+
return 0;
175+
}
176+
177+
stm32_pin_info pin_info = PIN_MAP[this->pin];
178+
uint16 compare = timer_get_compare(pin_info.timer_device,
179+
pin_info.timer_channel);
180+
181+
return COMPARE_TO_US(compare);
182+
}
183+
184+
void Servo::resetFields(void) {
185+
this->pin = NOT_ATTACHED;
186+
this->minAngle = MIN_ANGLE;
187+
this->maxAngle = MAX_ANGLE;
188+
this->minPW = MIN_PULSE_WIDTH;
189+
this->maxPW = MAX_PULSE_WIDTH;
190+
}

‎src/STM32F4/ServoTimers.h

Lines changed: 203 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,203 @@
1+
/******************************************************************************
2+
* The MIT License
3+
*
4+
* Copyright (c) 2010, LeafLabs, LLC.
5+
*
6+
* Permission is hereby granted, free of charge, to any person
7+
* obtaining a copy of this software and associated documentation
8+
* files (the "Software"), to deal in the Software without
9+
* restriction, including without limitation the rights to use, copy,
10+
* modify, merge, publish, distribute, sublicense, and/or sell copies
11+
* of the Software, and to permit persons to whom the Software is
12+
* furnished to do so, subject to the following conditions:
13+
*
14+
* The above copyright notice and this permission notice shall be
15+
* included in all copies or substantial portions of the Software.
16+
*
17+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
18+
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
19+
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
20+
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
21+
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
22+
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
23+
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24+
* SOFTWARE.
25+
*****************************************************************************/
26+
27+
/*
28+
* Arduino srl - www.arduino.org
29+
* 2017 Feb 23: Edited by Francesco Alessi (alfran) - [email protected]
30+
*/
31+
#ifndef _SERVO_H_
32+
#define _SERVO_H_
33+
34+
#include "types.h"
35+
#include "timer.h"
36+
37+
#include "wiring.h" /* hack for IDE compile */
38+
39+
/*
40+
* Note on Arduino compatibility:
41+
*
42+
* In the Arduino implementation, PWM is done "by hand" in the sense
43+
* that timer channels are hijacked in groups and an ISR is set which
44+
* toggles Servo::attach()ed pins using digitalWrite().
45+
*
46+
* While this scheme allows any pin to drive a servo, it chews up
47+
* cycles and complicates the programmer's notion of when a particular
48+
* timer channel will be in use.
49+
*
50+
* This implementation only allows Servo instances to attach() to pins
51+
* that already have a timer channel associated with them, and just
52+
* uses pwmWrite() to drive the wave.
53+
*
54+
* This introduces an incompatibility: while the Arduino
55+
* implementation of attach() returns the affected channel on success
56+
* and 0 on failure, this one returns true on success and false on
57+
* failure.
58+
*
59+
* RC Servos expect a pulse every 20ms. Since periods are set for
60+
* entire timers, rather than individual channels, attach()ing a Servo
61+
* to a pin can interfere with other pins associated with the same
62+
* timer. As always, your board's pin map is your friend.
63+
*/
64+
65+
// Pin number of unattached pins
66+
#define NOT_ATTACHED (-1)
67+
68+
// Default min/max pulse widths (in microseconds) and angles (in
69+
// degrees). Values chosen for Arduino compatibility. These values
70+
// are part of the public API; DO NOT CHANGE THEM.
71+
#define MIN_ANGLE 0
72+
#define MAX_ANGLE 180
73+
74+
#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo
75+
#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo
76+
77+
/** Class for interfacing with RC servomotors. */
78+
class Servo {
79+
public:
80+
/**
81+
* @brief Construct a new Servo instance.
82+
*
83+
* The new instance will not be attached to any pin.
84+
*/
85+
Servo();
86+
87+
/**
88+
* @brief Associate this instance with a servomotor whose input is
89+
* connected to pin.
90+
*
91+
* If this instance is already attached to a pin, it will be
92+
* detached before being attached to the new pin. This function
93+
* doesn't detach any interrupt attached with the pin's timer
94+
* channel.
95+
*
96+
* @param pin Pin connected to the servo pulse wave input. This
97+
* pin must be capable of PWM output.
98+
*
99+
* @param minPulseWidth Minimum pulse width to write to pin, in
100+
* microseconds. This will be associated
101+
* with a minAngle degree angle. Defaults to
102+
* SERVO_DEFAULT_MIN_PW = 544.
103+
*
104+
* @param maxPulseWidth Maximum pulse width to write to pin, in
105+
* microseconds. This will be associated
106+
* with a maxAngle degree angle. Defaults to
107+
* SERVO_DEFAULT_MAX_PW = 2400.
108+
*
109+
* @param minAngle Target angle (in degrees) associated with
110+
* minPulseWidth. Defaults to
111+
* SERVO_DEFAULT_MIN_ANGLE = 0.
112+
*
113+
* @param maxAngle Target angle (in degrees) associated with
114+
* maxPulseWidth. Defaults to
115+
* SERVO_DEFAULT_MAX_ANGLE = 180.
116+
*
117+
* @sideeffect May set pinMode(pin, PWM).
118+
*
119+
* @return true if successful, false when pin doesn't support PWM.
120+
*/
121+
122+
bool attach(uint8 pin,
123+
uint16 minPulseWidth=MIN_PULSE_WIDTH,
124+
uint16 maxPulseWidth=MAX_PULSE_WIDTH,
125+
int16 minAngle=MIN_ANGLE,
126+
int16 maxAngle=MAX_ANGLE);
127+
/**
128+
* @brief Stop driving the servo pulse train.
129+
*
130+
* If not currently attached to a motor, this function has no effect.
131+
*
132+
* @return true if this call did anything, false otherwise.
133+
*/
134+
bool detach();
135+
136+
/**
137+
* @brief Set the servomotor target angle.
138+
*
139+
* @param angle Target angle, in degrees. If the target angle is
140+
* outside the range specified at attach() time, it
141+
* will be clamped to lie in that range.
142+
*
143+
* @see Servo::attach()
144+
*/
145+
void write(int angle);
146+
147+
/**
148+
* @brief Set the pulse width, in microseconds.
149+
*
150+
* @param pulseWidth Pulse width to send to the servomotor, in
151+
* microseconds. If outside of the range
152+
* specified at attach() time, it is clamped to
153+
* lie in that range.
154+
*
155+
* @see Servo::attach()
156+
*/
157+
void writeMicroseconds(uint16 pulseWidth);
158+
159+
/**
160+
* Get the servomotor's target angle, in degrees. This will
161+
* lie inside the range specified at attach() time.
162+
*
163+
* @see Servo::attach()
164+
*/
165+
int read() const;
166+
167+
/**
168+
* Get the current pulse width, in microseconds. This will
169+
* lie within the range specified at attach() time.
170+
*
171+
* @see Servo::attach()
172+
*/
173+
uint16 readMicroseconds() const;
174+
175+
176+
/**
177+
* @brief Check if this instance is attached to a servo.
178+
* @return true if this instance is attached to a servo, false otherwise.
179+
* @see Servo::attachedPin()
180+
*/
181+
bool attached() const { return this->pin != NOT_ATTACHED; }
182+
183+
/**
184+
* @brief Get the pin this instance is attached to.
185+
* @return Pin number if currently attached to a pin, NOT_ATTACHED
186+
* otherwise.
187+
* @see Servo::attach()
188+
*/
189+
int attachedPin() const { return this->pin; }
190+
191+
private:
192+
int16 pin;
193+
uint16 minPW;
194+
uint16 maxPW;
195+
int16 minAngle;
196+
int16 maxAngle;
197+
198+
void resetFields(void);
199+
};
200+
201+
202+
203+
#endif /* _SERVO_H_ */

0 commit comments

Comments
 (0)