Skip to content

Commit 2ff4b32

Browse files
NathanPoNathanPo
committed
new architecture
1 parent e83f073 commit 2ff4b32

34 files changed

+2387
-0
lines changed

‎spider.ino ‎Home-made/spider.ino

File renamed without changes.

‎advanced-project/spider/arm.h

+157
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,157 @@
1+
#pragma once
2+
#include <Arduino.h>
3+
#include "utils/vectors.h"
4+
#include "kinematics.h"
5+
#include "motors.h"
6+
#include "spider_model.h"
7+
8+
#define KEEP 255
9+
10+
extern Motors<MOTORS_NUMBER> motors;
11+
12+
template<int NNode, typename KinematicsController = Kinematics3>
13+
class Arm {
14+
public:
15+
Arm() : speed_factor(100) {} //Default arm position
16+
17+
/*
18+
* Initialise the arm
19+
* ids_ - Motors controller indexes (0, N), corresponding to the defined arm
20+
* lengths_ - The length of the differents segments of this arm
21+
* arm_pos_ - The function pointer to transform servo from theorical model to spider model
22+
*/
23+
void init(int ids_[NNode],
24+
SpiderModel::Length<NNode> lengths_,
25+
void (*arm_pos_)(float angle[NNode]))
26+
{
27+
kinematics.setLength(lengths_);
28+
kinematics.setOrientation(arm_pos_);
29+
30+
for (int i = 0; i < NNode; i++) {
31+
ids[i] = ids_[i];
32+
}
33+
setCurrentPos({100, 70, 15});
34+
initiated = true;
35+
}
36+
37+
/*
38+
* Wrapper to call kinematics servo motors correction and save it in the servo correction array
39+
*/
40+
void correct(const Vectorf measured = {100, 70, 42}, const Vectorf real = {100, 70, 42}) {
41+
kinematics.computeError(measured, real);
42+
}
43+
44+
/*
45+
* Process the movement of the arm from the current position to the target
46+
* At a given call refreshrate to respect the speed
47+
*/
48+
bool process(double refreshrate) {
49+
bool finished = true;
50+
if (!initiated)
51+
return finished;
52+
Vectorf target_speed = target_position - current_position;
53+
double local_speed = abs(speed_factor) / (refreshrate/2.); //Speed in mm/s so divide for refreshrate
54+
if (target_speed.magnitude() > local_speed) {
55+
target_speed.normalise();
56+
target_speed *= local_speed;
57+
current_position += target_speed;
58+
finished = false;
59+
} else {
60+
current_position = target_position;
61+
finished = true;
62+
}
63+
64+
float servos[NNode];
65+
computeAngles(current_position, servos);
66+
setServos(servos);
67+
return finished;
68+
}
69+
70+
/*
71+
* Get the current intermediate position of the arm
72+
*/
73+
Vectorf getCurrentPos() const {
74+
return current_position;
75+
}
76+
77+
/*
78+
* Init the starting position of the arm
79+
*/
80+
void setCurrentPos(const Vectorf pos_) {
81+
current_position = pos_;
82+
target_position = pos_;
83+
}
84+
85+
/*
86+
* Set the new target for the arm for the next process
87+
*/
88+
void setTargetPos(const Vectorf pos_) {
89+
Vectorf newpos = pos_;
90+
if (newpos.x == KEEP)
91+
newpos.x = current_position.x;
92+
if (newpos.y == KEEP)
93+
newpos.y = current_position.y;
94+
if (newpos.z == KEEP)
95+
newpos.z = current_position.z;
96+
97+
target_position = newpos;
98+
}
99+
/*
100+
* Set the speed factor of the arm for the movement
101+
*/
102+
void setMoveSpeed(const double speed_) {
103+
speed_factor = speed_;
104+
}
105+
/*
106+
* Compute servo motors angle for a position
107+
* It include, cartesian to polar call
108+
* Servo motor correction offset
109+
* And remap to servo motor range via arm_pos function
110+
*/
111+
void computeAngles(Vectorf pos, float servos[NNode])
112+
{
113+
/*Serial.print(pos.x);
114+
Serial.print(" ");
115+
Serial.print(pos.y);
116+
Serial.print(" ");
117+
Serial.print(pos.z);
118+
Serial.println(" ");*/
119+
kinematics.cartesianToPolar(pos, servos);
120+
/*Serial.println();
121+
for (int i = 0; i < NNode; i++) {
122+
Serial.print(servos[i]);
123+
Serial.print(" ");
124+
}
125+
Serial.println();*/
126+
kinematics.applyCorrection(servos);
127+
/*for (int i = 0; i < NNode; i++) {
128+
Serial.print(servos[i]);
129+
Serial.print(" ");
130+
}
131+
Serial.println();*/
132+
kinematics.orientAngles(servos);
133+
/*for (int i = 0; i < NNode; i++) {
134+
Serial.print(servos[i]);
135+
Serial.print(" ");
136+
}
137+
Serial.println();
138+
Serial.println("-------------------------");*/
139+
}
140+
141+
private:
142+
Vectorf current_position;
143+
Vectorf target_position;
144+
float speed_factor;
145+
uint8_t ids[NNode];
146+
KinematicsController kinematics;
147+
bool initiated = false;
148+
149+
void setServos(float servos[NNode])
150+
{
151+
for (int i = 0; i < NNode; i++) {
152+
motors.setServo(ids[i], servos[i]);
153+
}
154+
}
155+
156+
157+
};

‎advanced-project/spider/armcontroller.h

+99
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
#pragma once
2+
3+
#include "utils/vectors.h"
4+
#include "motors.h"
5+
#include "arm.h"
6+
7+
#include "lib/ArduinoQueue/ArduinoQueue.h"
8+
9+
struct Order {
10+
enum:uint8_t{POS, WAIT} tag;
11+
uint8_t index; //Index number of the arm
12+
float speed; //Speed in mm/s
13+
Vectorf target; //position target in cm
14+
};
15+
16+
extern Motors<MOTORS_NUMBER> motorsController;
17+
18+
template<int NArm, int NNode>
19+
class ArmController {
20+
public:
21+
22+
/*
23+
* Constructor with the previously initialised instance of motor controller
24+
*/
25+
ArmController(Motors<NArm * NNode>& motors_)
26+
: motors(motors_)
27+
{}
28+
29+
/*
30+
* Return reference to arm at index
31+
*/
32+
Arm<NNode>& getArm(int index) {
33+
return arms[index];
34+
}
35+
36+
/*
37+
* Add an order to the queue to be processed.
38+
* The order is specified before.
39+
*/
40+
void addPosition(Order ord) {
41+
q.enqueue(ord);
42+
}
43+
44+
/*
45+
* Process all currently queued orders. Respecting the speed time defined
46+
*/
47+
void process_orders() {
48+
//Pop angles, speed for motors to execute
49+
50+
while (pop_orders()) {
51+
unsigned long mtime = micros();
52+
sei();
53+
while (!execute_orders(micros() - mtime)) {
54+
mtime = micros();
55+
//delay(1);
56+
delayMicroseconds(500);
57+
}
58+
}
59+
}
60+
61+
private:
62+
Motors<NArm * NNode>& motors;
63+
Arm<NNode> arms[NArm];
64+
65+
ArduinoQueue<Order> q{100};
66+
67+
/*
68+
* Dequeue orders and apply it to the correct arm until a wait order or end of queue
69+
*/
70+
bool pop_orders() {
71+
if (q.isEmpty())
72+
return false;
73+
74+
while (!q.isEmpty()) {
75+
Order ord = q.dequeue();
76+
if (ord.tag == Order::WAIT)
77+
break;
78+
arms[ord.index].setTargetPos(ord.target);
79+
//if (ord.speed > 0)
80+
arms[ord.index].setMoveSpeed(ord.speed);
81+
}
82+
83+
return true;
84+
}
85+
86+
/*
87+
* Execute orders applyed for each arm
88+
* Return when all arm reached their target
89+
*/
90+
bool execute_orders(double elapsed_time) {
91+
bool arrived = true;
92+
for (int i = 0; i < NArm; i++) {
93+
if (!arms[i].process(1000000./elapsed_time))
94+
arrived = false;
95+
}
96+
return arrived;
97+
}
98+
99+
};

0 commit comments

Comments
 (0)