Skip to content

Commit 28e4bf4

Browse files
facchinmfacchinm
committed
Add D11 firmware
1 parent 4fa9bd3 commit 28e4bf4

40 files changed

+4078
-0
lines changed

‎extra/D11-Firmware/Battery.cpp

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
#include "Battery.h"
2+
#include "Events.h"
3+
#include "Wire.h"
4+
5+
#define SCALE_FACTOR (77)
6+
7+
void Battery::readBattery() {
8+
readBuf[index % 10] = (((int)analogRead(pin)) / SCALE_FACTOR);
9+
index ++;
10+
}
11+
12+
void readBattery_wrapper(void* arg)
13+
{
14+
Battery* obj = (Battery*)arg;
15+
obj->readBattery();
16+
}
17+
18+
Battery::Battery(int pinA) {
19+
pin = pinA;
20+
registerTimedEvent(readBattery_wrapper, this, 1000);
21+
}
22+
23+
void Battery::getRaw() {
24+
Wire.write((int)analogRead(pin));
25+
}
26+
27+
void Battery::getConverted() {
28+
Wire.write((int)(analogRead(pin) / SCALE_FACTOR));
29+
}
30+
31+
void Battery::getFiltered() {
32+
long total = 0;
33+
for (int i = 0; i < 10; i++) {
34+
total += readBuf[i];
35+
}
36+
Wire.write((int)(total/10));
37+
}

‎extra/D11-Firmware/Battery.h

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
#include "Arduino.h"
2+
3+
class Battery {
4+
public:
5+
Battery(int pinA);
6+
void getRaw();
7+
void getConverted();
8+
void getFiltered();
9+
void readBattery();
10+
private:
11+
int pin;
12+
int readBuf[10];
13+
uint8_t index;
14+
};

‎extra/D11-Firmware/Common.h

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
enum Commands {
2+
GET_VERSION = 0x01,
3+
RESET,
4+
SET_PWM_DUTY_CYCLE_SERVO,
5+
SET_PWM_FREQUENCY_SERVO,
6+
SET_PWM_DUTY_CYCLE_DC_MOTOR,
7+
SET_PWM_FREQUENCY_DC_MOTOR,
8+
GET_RAW_COUNT_ENCODER,
9+
RESET_COUNT_ENCODER,
10+
GET_OVERFLOW_UNDERFLOW_STATUS_ENCODER,
11+
GET_COUNT_PER_SECOND_ENCODER,
12+
SET_INTERRUPT_ON_COUNT_ENCODER,
13+
SET_INTERRUPT_ON_VELOCITY_ENCODER,
14+
GET_RAW_ADC_BATTERY,
15+
GET_CONVERTED_ADC_BATTERY,
16+
GET_FILTERED_ADC_BATTERY,
17+
SET_PID_GAIN_CL_MOTOR,
18+
RESET_PID_GAIN_CL_MOTOR,
19+
SET_CONTROL_MODE_CL_MOTOR,
20+
SET_POSITION_SETPOINT_CL_MOTOR,
21+
SET_VELOCITY_SETPOINT_CL_MOTOR,
22+
SET_MAX_ACCELERATION_CL_MOTOR,
23+
SET_MAX_VELOCITY_CL_MOTOR,
24+
SET_MIN_MAX_DUTY_CYCLE_CL_MOTOR,
25+
PING,
26+
GET_INTERNAL_TEMP,
27+
};
28+
29+
enum IRQCause {
30+
ENCODER_COUNTER_REACHED = 1,
31+
ENCODER_VELOCITY_REACHED,
32+
};
33+
34+
#define I2C_ADDRESS 0x66
35+
36+
#define ADC_BATTERY A2
37+
#define IRQ_PIN 27
38+
#define LED_BUILTIN 3
39+
40+
#define MOTOR_2_COUNTER TC1
41+
#define MOTOR_2_PIN_A 4
42+
#define MOTOR_2_PIN_B 5
43+
44+
#define MOTOR_1_COUNTER TC2
45+
#define MOTOR_1_PIN_A 7
46+
#define MOTOR_1_PIN_B 6
47+
48+
#define PWM_PIN_SERVO_COUNTER TCC0
49+
50+
#define PWM_PIN_SERVO_1 17
51+
#define PWM_PIN_SERVO_2 23
52+
#define PWM_PIN_SERVO_3 16
53+
#define PWM_PIN_SERVO_4 22
54+
55+
#define ENCODER_1_PIN_A 9
56+
#define ENCODER_1_PIN_B 8
57+
#define ENCODER_2_PIN_A 10
58+
#define ENCODER_2_PIN_B 11
59+
60+
void requestAttention(int cause);

‎extra/D11-Firmware/D11-Firmware.ino

Lines changed: 215 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,215 @@
1+
#include <Wire.h>
2+
#include "EncoderWrapper.h"
3+
#include "Battery.h"
4+
#include "DCMotor.h"
5+
#include "ServoMotor.h"
6+
#include "Common.h"
7+
#include "Events.h"
8+
#include "PID.h"
9+
10+
#define Fix16 mn::MFixedPoint::FpF32<8>
11+
12+
// compile me with target mkrmotorshield:samd:mkrmotorshield:bootloader=0kb,pinmap=complete,lto=disabled during development
13+
// compile me with target mkrmotorshield:samd:mkrmotorshield:bootloader=4kb,pinmap=complete,lto=enabled for release
14+
15+
const char* FW_VERSION = "0.06";
16+
17+
DCMotor dcmotors[2] = {
18+
DCMotor(MOTOR_1_COUNTER, MOTOR_1_PIN_A, MOTOR_1_PIN_B),
19+
DCMotor(MOTOR_2_COUNTER, MOTOR_2_PIN_A, MOTOR_2_PIN_B),
20+
};
21+
22+
ServoMotor servos[4] = {
23+
ServoMotor(PWM_PIN_SERVO_1),
24+
ServoMotor(PWM_PIN_SERVO_2),
25+
ServoMotor(PWM_PIN_SERVO_3),
26+
ServoMotor(PWM_PIN_SERVO_4),
27+
};
28+
29+
EncoderWrapper encoders[2] = {
30+
EncoderWrapper(ENCODER_2_PIN_A, ENCODER_2_PIN_B, 1),
31+
EncoderWrapper(ENCODER_1_PIN_A, ENCODER_1_PIN_B, 0),
32+
};
33+
34+
PIDWrapper pid_control[2] = {
35+
PIDWrapper(encoders[0].position, encoders[0].velocity, &dcmotors[0], 0, 10, 100), //10ms period velo, 100ms period pos
36+
PIDWrapper(encoders[1].position, encoders[1].velocity, &dcmotors[1], 1, 10, 100),
37+
};
38+
39+
Battery battery(ADC_BATTERY);
40+
41+
void led_on() {
42+
digitalWrite(LED_BUILTIN, HIGH);
43+
}
44+
45+
void setup() {
46+
47+
WDT->CTRL.reg &= ~WDT_CTRL_ENABLE;
48+
while (WDT->STATUS.reg & WDT_STATUS_SYNCBUSY);
49+
50+
temp_init();
51+
52+
Wire.begin(I2C_ADDRESS);
53+
Wire.onRequest(requestEvent);
54+
Wire.onReceive(receiveEvent);
55+
pinMode(LED_BUILTIN, OUTPUT);
56+
pinMode(IRQ_PIN, OUTPUT);
57+
analogWriteResolution(8);
58+
}
59+
60+
volatile uint8_t command = 0;
61+
volatile uint8_t target = 0;
62+
volatile uint8_t irq_status = 0;
63+
volatile unsigned long nextTimedEvent = 0;
64+
volatile unsigned long lastMessageReceived = 0;
65+
66+
bool irq_enabled = true;
67+
68+
void loop() {
69+
if (command == RESET || ((lastMessageReceived != 0) && (millis() - lastMessageReceived > 10000))) {
70+
reboot();
71+
}
72+
if (command == PING) {
73+
lastMessageReceived = millis();
74+
}
75+
executeTimedEvents();
76+
}
77+
78+
void receiveEvent(int howMany) {
79+
command = Wire.read();
80+
if (Wire.available()) {
81+
target = Wire.read();
82+
} else {
83+
return;
84+
}
85+
int value = 0;
86+
87+
88+
if (!Wire.available()) {
89+
return;
90+
}
91+
92+
uint8_t buf[8];
93+
int i = 0;
94+
while (Wire.available()) {
95+
buf[i++] = (uint8_t)Wire.read();
96+
}
97+
98+
// copies the bytes to int
99+
memcpy(&value, buf, sizeof(value));
100+
101+
switch (command) {
102+
case SET_PWM_DUTY_CYCLE_SERVO:
103+
servos[target].setDuty(value);
104+
break;
105+
case SET_PWM_FREQUENCY_SERVO:
106+
servos[target].setFrequency(value);
107+
break;
108+
case SET_PWM_DUTY_CYCLE_DC_MOTOR:
109+
((PIDWrapper*)dcmotors[target].pid)->stop();
110+
dcmotors[target].setDuty(value);
111+
break;
112+
case SET_PWM_FREQUENCY_DC_MOTOR:
113+
dcmotors[target].setFrequency(value);
114+
break;
115+
case RESET_COUNT_ENCODER:
116+
encoders[target].resetCounter(value);
117+
break;
118+
case SET_INTERRUPT_ON_COUNT_ENCODER:
119+
encoders[target].setIrqOnCount(value);
120+
break;
121+
case SET_INTERRUPT_ON_VELOCITY_ENCODER:
122+
encoders[target].setIrqOnVelocity(value);
123+
break;
124+
case SET_PID_GAIN_CL_MOTOR:
125+
{
126+
int16_t P16 = *((int16_t*)&buf[0]);
127+
int16_t I16 = *((int16_t*)&buf[2]);
128+
int16_t D16 = *((int16_t*)&buf[4]);
129+
Fix16 P = ((Fix16)P16) / short(1000);
130+
Fix16 I = ((Fix16)I16) / short(1000);
131+
Fix16 D = ((Fix16)D16) / short(1000);
132+
pid_control[target].setGains(P, I , D);
133+
break;
134+
}
135+
case RESET_PID_GAIN_CL_MOTOR:
136+
pid_control[target].resetGains();
137+
break;
138+
case SET_CONTROL_MODE_CL_MOTOR:
139+
pid_control[target].setControlMode((cl_control)value);
140+
break;
141+
case SET_POSITION_SETPOINT_CL_MOTOR:
142+
pid_control[target].setSetpoint(TARGET_POSITION, Fix16(value * 1.0));
143+
break;
144+
case SET_VELOCITY_SETPOINT_CL_MOTOR:
145+
pid_control[target].setSetpoint(TARGET_VELOCITY, Fix16(value * 1.0));
146+
break;
147+
case SET_MAX_ACCELERATION_CL_MOTOR: {
148+
pid_control[target].setMaxAcceleration(Fix16(value * 1.0));
149+
break;
150+
}
151+
case SET_MAX_VELOCITY_CL_MOTOR:
152+
pid_control[target].setMaxVelocity(Fix16(value * 1.0));
153+
break;
154+
case SET_MIN_MAX_DUTY_CYCLE_CL_MOTOR:
155+
pid_control[target].setLimits(*((int16_t*)&buf[0]), *((int16_t*)&buf[2]));
156+
break;
157+
}
158+
}
159+
160+
void requestEvent() {
161+
//deassert IRQ
162+
if (irq_enabled) {
163+
digitalWrite(IRQ_PIN, HIGH);
164+
}
165+
166+
// Always reply with irq status
167+
Wire.write(irq_status);
168+
irq_status = 0;
169+
170+
switch (command) {
171+
case GET_VERSION:
172+
getFWVersion();
173+
break;
174+
case GET_RAW_COUNT_ENCODER:
175+
encoders[target].getRawCount();
176+
break;
177+
case GET_OVERFLOW_UNDERFLOW_STATUS_ENCODER:
178+
encoders[target].getOverflowUnderflow();
179+
break;
180+
case GET_COUNT_PER_SECOND_ENCODER:
181+
encoders[target].getCountPerSecond();
182+
break;
183+
case GET_RAW_ADC_BATTERY:
184+
battery.getRaw();
185+
break;
186+
case GET_CONVERTED_ADC_BATTERY:
187+
battery.getConverted();
188+
break;
189+
case GET_FILTERED_ADC_BATTERY:
190+
battery.getFiltered();
191+
break;
192+
case GET_INTERNAL_TEMP:
193+
getInternalTemperature();
194+
break;
195+
}
196+
}
197+
198+
void requestAttention(int cause) {
199+
if (irq_enabled) {
200+
digitalWrite(IRQ_PIN, LOW);
201+
}
202+
irq_status |= (1 << cause);
203+
}
204+
205+
void getFWVersion() {
206+
Wire.write(FW_VERSION);
207+
}
208+
209+
void getInternalTemperature() {
210+
Wire.write(temp_raw_to_mdeg(temp_read_raw()));
211+
}
212+
213+
void reboot() {
214+
NVIC_SystemReset();
215+
}

‎extra/D11-Firmware/DCMotor.cpp

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
#include "DCMotor.h"
2+
3+
void DCMotor::setDuty(int duty) {
4+
this->duty = duty;
5+
if (duty == 0) {
6+
analogWrite(in1, 0);
7+
analogWrite(in2, 0);
8+
return;
9+
}
10+
11+
// scale duty to period
12+
duty = duty * 255 / 100;
13+
14+
if (duty > 0) {
15+
analogWrite(in1, 0);
16+
analogWrite(in2, duty);
17+
} else {
18+
analogWrite(in2, 0);
19+
analogWrite(in1, -duty);
20+
}
21+
}
22+
23+
void DCMotor::setFrequency(int frequency) {
24+
// NB: not implemented at the moment!
25+
//this->frequency = frequency;
26+
}

‎extra/D11-Firmware/DCMotor.h

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
#ifndef __DC_MOTOR_H__
2+
#define __DC_MOTOR_H__
3+
4+
#include "Arduino.h"
5+
6+
class DCMotor {
7+
public:
8+
DCMotor(Tc* tcc, int pinA, int pinB) : in1(pinA), in2(pinB) {
9+
pinMode(in1, OUTPUT);
10+
pinMode(in2, OUTPUT);
11+
};
12+
void setDuty(int duty);
13+
void setFrequency(int frequency);
14+
int duty = 0;
15+
void* pid;
16+
private:
17+
int in1;
18+
int in2;
19+
int frequency = 100;
20+
};
21+
22+
#endif

0 commit comments

Comments
 (0)