openDogV3

1
// init hips
2

3
void OdriveInit1() {
4

5
      Serial.println("ODrive 1");
6

7
      for (int axis = 0; axis < 2; ++axis) {
8
          //Serial1 << "w axis" << axis << ".controller.config.vel_limit " << 6000.0f << '\n';          // *** Velocity limits should be set to infinite through the ODrive tool, this stops the motors disarming under certain situations ***
9
          Serial1 << "w axis" << axis << ".motor.config.current_lim " << 20.0f << '\n';
10
  
11
          requested_state = requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL;
12
          Serial << "Axis" << axis << ": Requesting state " << requested_state << '\n';
13
          odrive1.run_state(axis, requested_state, false); // don't wait 
14
      }      
15

16
     Serial.println("ODrive 2");
17

18
      for (int axis = 0; axis < 2; ++axis) {
19
          //Serial2 << "w axis" << axis << ".controller.config.vel_limit " << 6000.0f << '\n';
20
          Serial2 << "w axis" << axis << ".motor.config.current_lim " << 20.0f << '\n';
21
  
22
          requested_state = requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL;
23
          Serial2 << "Axis" << axis << ": Requesting state " << requested_state << '\n';
24
          odrive2.run_state(axis, requested_state, false); // don't wait 
25
      }   
26

27
      Serial.println("ODrive 3");
28

29
      for (int axis = 0; axis < 2; ++axis) {
30
          //Serial3 << "w axis" << axis << ".controller.config.vel_limit " << 6000.0f << '\n';
31
          Serial3 << "w axis" << axis << ".motor.config.current_lim " << 20.0f << '\n';
32
  
33
          requested_state = requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL;
34
          Serial3 << "Axis" << axis << ": Requesting state " << requested_state << '\n';
35
          odrive3.run_state(axis, requested_state, false); // don't wait 
36
      } 
37

38
      Serial.println("ODrive 4");
39

40
      for (int axis = 0; axis < 2; ++axis) {
41
          //Serial4 << "w axis" << axis << ".controller.config.vel_limit " << 6000.0f << '\n';
42
          Serial4 << "w axis" << axis << ".motor.config.current_lim " << 20.0f << '\n';
43
  
44
          requested_state = requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL;
45
          Serial4 << "Axis" << axis << ": Requesting state " << requested_state << '\n';
46
          odrive4.run_state(axis, requested_state, false); // don't wait 
47
      }
48

49
      Serial.println("ODrive 5");
50

51
      for (int axis = 0; axis < 2; ++axis) {
52
          //Serial5 << "w axis" << axis << ".controller.config.vel_limit " << 6000.0f << '\n';
53
          Serial5 << "w axis" << axis << ".motor.config.current_lim " << 20.0f << '\n';
54
  
55
          requested_state = requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL;
56
          Serial5 << "Axis" << axis << ": Requesting state " << requested_state << '\n';
57
          odrive5.run_state(axis, requested_state, false); // don't wait 
58
      }
59

60
      Serial.println("ODrive 6");
61

62
      for (int axis = 0; axis < 2; ++axis) {
63
          //Serial6 << "w axis" << axis << ".controller.config.vel_limit " << 6000.0f << '\n';
64
          Serial6 << "w axis" << axis << ".motor.config.current_lim " << 20.0f << '\n';
65
  
66
          requested_state = requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL;
67
          Serial6 << "Axis" << axis << ": Requesting state " << requested_state << '\n';
68
          odrive6.run_state(axis, requested_state, false); // don't wait 
69
      }
70
}
71

72

73
void modifyGains() {                               // this function turns up the gains when it is executed (menu option 4 via the remote)
74

75
          Serial.println("modfy gains");
76

77
          float posGainKnee = 15.0;
78
          float posGainHips = 70.0;  
79
          float posGainShoulder = 15.0; 
80
          float velGain = 0.1;      
81
          float integrator = 0.1;  
82

83
          Serial1 << "w axis" << 0 << ".controller.config.pos_gain " << posGainHips << '\n';
84
          Serial1 << "w axis" << 1 << ".controller.config.pos_gain " << posGainHips << '\n';
85

86
          Serial2 << "w axis" << 0 << ".controller.config.pos_gain " << posGainKnee << '\n';
87
          Serial2 << "w axis" << 1 << ".controller.config.pos_gain " << posGainShoulder << '\n';
88

89
          Serial3 << "w axis" << 0 << ".controller.config.pos_gain " << posGainKnee << '\n';
90
          Serial3 << "w axis" << 1 << ".controller.config.pos_gain " << posGainShoulder << '\n';
91

92
          Serial4 << "w axis" << 0 << ".controller.config.pos_gain " << posGainHips << '\n';
93
          Serial4 << "w axis" << 1 << ".controller.config.pos_gain " << posGainHips << '\n';
94

95
          Serial5 << "w axis" << 0 << ".controller.config.pos_gain " << posGainKnee << '\n';
96
          Serial5 << "w axis" << 1 << ".controller.config.pos_gain " << posGainShoulder << '\n';
97

98
          Serial6 << "w axis" << 0 << ".controller.config.pos_gain " << posGainKnee << '\n';
99
          Serial6 << "w axis" << 1 << ".controller.config.pos_gain " << posGainShoulder << '\n';
100

101
          // ******
102

103
          Serial1 << "w axis" << 0 << ".controller.config.vel_gain " << velGain << '\n';
104
          Serial1 << "w axis" << 1 << ".controller.config.vel_gain " << velGain << '\n';
105

106
          Serial2 << "w axis" << 0 << ".controller.config.vel_gain " << velGain << '\n';
107
          Serial2 << "w axis" << 1 << ".controller.config.vel_gain " << velGain << '\n';
108

109
          Serial3 << "w axis" << 0 << ".controller.config.vel_gain " << velGain << '\n';
110
          Serial3 << "w axis" << 1 << ".controller.config.vel_gain " << velGain << '\n';
111

112
          Serial4 << "w axis" << 0 << ".controller.config.vel_gain " << velGain << '\n';
113
          Serial4 << "w axis" << 1 << ".controller.config.vel_gain " << velGain << '\n';
114

115
          Serial5 << "w axis" << 0 << ".controller.config.vel_gain " << velGain << '\n';
116
          Serial5 << "w axis" << 1 << ".controller.config.vel_gain " << velGain << '\n';
117

118
          Serial6 << "w axis" << 0 << ".controller.config.vel_gain " << velGain << '\n';
119
          Serial6 << "w axis" << 1 << ".controller.config.vel_gain " << velGain << '\n';
120

121
          // ******
122

123
          Serial1 << "w axis" << 0 << ".controller.config.vel_integrator_gain " << integrator << '\n';
124
          Serial1 << "w axis" << 1 << ".controller.config.vel_integrator_gain " << integrator << '\n';
125

126
          Serial2 << "w axis" << 0 << ".controller.config.vel_integrator_gain " << integrator << '\n';
127
          Serial2 << "w axis" << 1 << ".controller.config.vel_integrator_gain " << integrator << '\n';
128

129
          Serial3 << "w axis" << 0 << ".controller.config.vel_integrator_gain " << integrator << '\n';
130
          Serial3 << "w axis" << 1 << ".controller.config.vel_integrator_gain " << integrator << '\n';
131

132
          Serial4 << "w axis" << 0 << ".controller.config.vel_integrator_gain " << integrator << '\n';
133
          Serial4 << "w axis" << 1 << ".controller.config.vel_integrator_gain " << integrator << '\n';
134

135
          Serial5 << "w axis" << 0 << ".controller.config.vel_integrator_gain " << integrator << '\n';
136
          Serial5 << "w axis" << 1 << ".controller.config.vel_integrator_gain " << integrator << '\n';
137

138
          Serial6 << "w axis" << 0 << ".controller.config.vel_integrator_gain " << integrator << '\n';
139
          Serial6 << "w axis" << 1 << ".controller.config.vel_integrator_gain " << integrator << '\n';        
140
}
141

142
void applyOffsets1() {
143
          // apply initial offsets to ODrives for hips         
144
          
145
          odrive1.SetPosition(0, offSet10);         // hips - right front
146
          odrive1.SetPosition(1, offSet11);         // hips - right rear
147
          odrive4.SetPosition(0, offSet40);         // hips - left front
148
          odrive4.SetPosition(1, offSet41);         // hips - left rear 
149
}
150

151
void applyOffsets2() {
152
          // apply initial offsets to ODrives for shoulders
153

154
          odrive2.SetPosition(1, offSet21);         // shoulder - right front
155
          odrive3.SetPosition(1, offSet31);         // shoulder - right rear       
156
          odrive5.SetPosition(1, offSet51);         // shoulder - left front
157
          odrive6.SetPosition(1, offSet61);         // shoulder - left rear 
158
          odrive2.SetPosition(0, offSet20);         // knee - right front
159
          odrive3.SetPosition(0, offSet30);         // knee - right rear        
160
          odrive5.SetPosition(0, offSet50);         // knee - left front
161
          odrive6.SetPosition(0, offSet60);         // knee - left front   
162
              
163
}
164

165

166

167

168
void driveJoints(int joint, float pos) {
169
          // takes into account the original setup offsets for motor postions, and also turns around directions so they are consistent
170
          // also constrains the motion limts for each joint    
171

172
          if (mydata_remote.toggleTop == 1) {       // ************** only do it if the motor enable is on via teh remote *****************
173

174
              // knees
175

176
              if (joint == 20) {
177
                  pos = constrain(pos, -2.5,2.5);
178
                  odrive2.SetPosition(0, pos + offSet20);    // knee - right front
179
              }
180
              else if (joint == 30) {
181
                  pos = constrain(pos, -2.5,2.5);
182
                  odrive3.SetPosition(0, (pos*-1) + offSet30);    // knee - right back
183
              }
184
              else if (joint == 50) {
185
                  pos = constrain(pos, -2.5,2.5);
186
                  odrive5.SetPosition(0, (pos*-1) + offSet50);    // knee - left front
187
              }
188
              else if (joint == 60) {
189
                  pos = constrain(pos, -2.5,2.5);
190
                  odrive6.SetPosition(0, pos + offSet60);    // knee - left back
191
              }
192

193
              // shoulder
194

195
              else if (joint == 21) {
196
                  pos = constrain(pos, -2.5,2.5);
197
                  odrive2.SetPosition(1, (pos*-1) + offSet21);    // shoulder - right front
198
              }        
199
              else if (joint == 31) {
200
                  pos = constrain(pos, -2.5,2.5);
201
                  odrive3.SetPosition(1, pos + offSet31);    // shoulder - right rear
202
              }        
203
              else if (joint == 51) {
204
                  pos = constrain(pos, -2.5,2.5);
205
                  odrive5.SetPosition(1, pos + offSet51);    // shoulder - left front
206
              }        
207
              else if (joint == 61) {
208
                  pos = constrain(pos, -2.5,2.5);
209
                  odrive6.SetPosition(1, (pos*-1) + offSet61);    // shoulder - left rear      
210
              }
211

212
              // hips
213
              else if (joint == 10) {
214
                  pos = constrain(pos, -2.5,2.5);
215
                  odrive1.SetPosition(0, pos+offSet10);    // hips - right front
216
              }
217
              else if (joint == 11) {
218
                  pos = constrain(pos, -2.5,2.5);
219
                  odrive1.SetPosition(1, (pos*-1)+offSet11);    // hips - right rear
220
              }
221
              else if (joint == 40) {
222
                  pos = constrain(pos, -2.5,2.5);
223
                  odrive4.SetPosition(0, pos+offSet40);    // hips - knee - left front
224
              }
225
              else if (joint == 41) {
226
                  pos = constrain(pos, -2.5,2.5);
227
                  odrive4.SetPosition(1, (pos*-1)+offSet41);    // hips - left rear
228
              }
229
     
230

231
          }
232

233
          
234

235
          
236
}
237