robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions

ADDED: naive control implementation from RO2 for a robot roller motor

Browse source
This commit is contained in:
vanyabeat 2024-05-24 10:57:24 +00:00 • committed by Igor Brylyov
parent e876d7e56c
commit 36253b1db5
24 changed files with 819 additions and 100 deletions
Download patch file Download diff file Expand all files Collapse all files

29
firmware/embed/src/main.cpp
View file

@ -68,33 +68,21 @@ void setup_foc(MagneticSensorAS5045 *sensor, BLDCMotor *motor, DRV8313Driver *dr
motor->initFOC();
}
uint8_t Counter;
void send_data() {
// TODO(vanyabeat) add can functionability for ROS packets
if (Counter >= 255) {
Counter = 0;
}
CAN_TX_msg.id = 1;
snprintf(reinterpret_cast<char *>(CAN_TX_msg.buf), sizeof(CAN_TX_msg.buf), "CNT:%d", Counter);
CAN_TX_msg.len = 8;
CAN_TX_msg.buf[0] = 'A';
CAN_TX_msg.len = 5;
Can.write(CAN_TX_msg);
// ++Counter;
// Serial.print("Sent: \n");
digitalWrite(PC11, !digitalRead(PC11));
}
void read_can_message() {
Serial.print("Received: ");
Serial.print(CAN_inMsg.id);
Serial.print(" ");
Serial.print(CAN_inMsg.len);
Serial.print(" ");
CAN_inMsg.buf[7] = 0;
auto ptr = reinterpret_cast<char *>(CAN_inMsg.buf);
Serial.println(ptr);
doMotor(ptr);
Serial.print("\n");
float angle;
memcpy(&angle, &CAN_inMsg.buf[1], sizeof(angle));
motor.target = angle;
digitalWrite(PC10, !digitalRead(PC10));
}
void run_foc(BLDCMotor *motor, Commander *commander) {
@ -115,15 +103,18 @@ void setup() {
Can.setBaudRate(1000000);
TIM_TypeDef *Instance = TIM2;
HardwareTimer *SendTimer = new HardwareTimer(Instance);
SendTimer->setOverflow(1, HERTZ_FORMAT); // 50 Hz
SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz
SendTimer->attachInterrupt(send_data);
SendTimer->resume();
setup_foc(&sensor, &motor, &driver, &current_sense, &command, doMotor);
_delay(1000);
}
float current_angle = 0;
void loop() {
run_foc(&motor, &command);
current_angle = sensor.getAngle();
memcpy(&CAN_TX_msg.buf[1], &current_angle, sizeof(current_angle));
while (Can.read(CAN_inMsg)) {
read_can_message();
}