sixiDriver/sixiDriver.h at main · PolyOrbite-Rover/sixiDriver · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
/**************************************
 * Title    : sixiDriver.h
 * Author   : Ian Gagnon
 * Created  : 2021-04-21
 *************************************/

// Motor gearbox ratio
#define MOTOR_0_GEARBOX_RATIO 30.0
#define MOTOR_1_GEARBOX_RATIO 40.0
#define MOTOR_2_GEARBOX_RATIO 35.0
#define MOTOR_3_GEARBOX_RATIO 20.0 // To validate
#define MOTOR_4_GEARBOX_RATIO 20.0 // To validate
#define MOTOR_5_GEARBOX_RATIO 20.0 // To validate

// Controller steps per revolution (check controller settings)
#define CONTROLLER_0_STEPS_PER_REV 400.0
#define CONTROLLER_1_STEPS_PER_REV 400.0
#define CONTROLLER_2_STEPS_PER_REV 400.0
#define CONTROLLER_3_STEPS_PER_REV 400.0 // To validate
#define CONTROLLER_4_STEPS_PER_REV 400.0 // To validate
#define CONTROLLER_5_STEPS_PER_REV 400.0 // To validate

// Motor steps per degree
#define MOTOR_0_STEPS_PER_DEGREE (CONTROLLER_0_STEPS_PER_REV * MOTOR_0_GEARBOX_RATIO) / 360.0
#define MOTOR_1_STEPS_PER_DEGREE (CONTROLLER_1_STEPS_PER_REV * MOTOR_1_GEARBOX_RATIO) / 360.0
#define MOTOR_2_STEPS_PER_DEGREE (CONTROLLER_2_STEPS_PER_REV * MOTOR_2_GEARBOX_RATIO) / 360.0
#define MOTOR_3_STEPS_PER_DEGREE (CONTROLLER_3_STEPS_PER_REV * MOTOR_3_GEARBOX_RATIO) / 360.0 // To validate
#define MOTOR_4_STEPS_PER_DEGREE (CONTROLLER_4_STEPS_PER_REV * MOTOR_4_GEARBOX_RATIO) / 360.0 // To validate
#define MOTOR_5_STEPS_PER_DEGREE (CONTROLLER_5_STEPS_PER_REV * MOTOR_5_GEARBOX_RATIO) / 360.0 // To validate

#ifndef SIXI_H
#define SIXI_H

#include <AccelStepper.h>
#include <Servo.h>
#include "sixi_mega.h"
#include "rosserialInterface.h"
#include "sensorManager.h"

#define NUM_STEPPERS 6
#define NUM_SERVO 1

class SixiDriver
/**
 * @brief Driver for Sixi 2 robotic arm based on AccelStepper library
 * Uses rosserialInterface to communicates with ROS and sensorManager to read sensors
 */
{
public:
  // CTOR & DTOR
  SixiDriver(unsigned long publicationPeriodMs, bool verbose=false);
  ~SixiDriver();

  // Methods
  void start();
  void moveToHome();
  void updateSteppersPositions();
  void updateSensors();
  void enableMotors();
  void disableMotors();
  void setMaxSpeedForAll(const float* speedArray);
  void setAccelerationForAll(const float* accelArray);
  void moveAllTo(float* positionArray);
  void attachTool();
  void moveTool(int positionTool);
  void runAll();
  void main();

private:
  // Objects
  SensorManager sensorManager_;
  ROSserialInterface rosserialInterface_;
  AccelStepper* pAccelStepperObjectArray_[NUM_STEPPERS];
  Servo servoTool_;

  // Attributes
  float positions_[NUM_SENSORS];
  unsigned long lastTime_ = 0;
  unsigned long publicationPeriodMs_;
  bool verbose_;

  // Constants
  const int SERVO_PIN_= TOOL_PIN;

  const float STEPS_PER_DEGREE_[NUM_STEPPERS] = { MOTOR_0_STEPS_PER_DEGREE,
                                                  MOTOR_1_STEPS_PER_DEGREE,
                                                  MOTOR_2_STEPS_PER_DEGREE,
                                                  MOTOR_3_STEPS_PER_DEGREE,
                                                  MOTOR_4_STEPS_PER_DEGREE,
                                                  MOTOR_5_STEPS_PER_DEGREE};

  const int STEP_PINS_[NUM_STEPPERS] = {MOTOR_0_STEP_PIN,
                                        MOTOR_1_STEP_PIN,
                                        MOTOR_2_STEP_PIN,
                                        MOTOR_3_STEP_PIN,
                                        MOTOR_4_STEP_PIN,
                                        MOTOR_5_STEP_PIN};

  const int DIR_PINS_[NUM_STEPPERS] = { MOTOR_0_DIR_PIN,
                                        MOTOR_1_DIR_PIN,
                                        MOTOR_2_DIR_PIN,
                                        MOTOR_3_DIR_PIN,
                                        MOTOR_4_DIR_PIN,
                                        MOTOR_5_DIR_PIN};

  const int ENABLE_PINS_[NUM_STEPPERS] = {MOTOR_0_ENABLE_PIN,
                                          MOTOR_1_ENABLE_PIN,
                                          MOTOR_2_ENABLE_PIN,
                                          MOTOR_3_ENABLE_PIN,
                                          MOTOR_4_ENABLE_PIN,
                                          MOTOR_5_ENABLE_PIN};

  const float DEFAULT_SPEEDS_[NUM_STEPPERS] = {700.0,700.0, 700.0, 700.0, 700.0, 700.0};
  const float DEFAULT_ACCELS_[NUM_STEPPERS] = {300.0, 300.0, 300.0, 300.0, 300.0, 300.0};

  float HOME_POSITION_[NUM_STEPPERS] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

};
#endif