screwbike

005.ino
653 строки · 21.1 Кб
Перенос по словам
1
#include <Arduino.h>
2
#include "ODriveCAN.h"
3

4
//PID
5
#include <PID_v1.h>
6

7
// PID1
8
double Pk1 = 4.5; 
9
double Ik1 = 12;
10
double Dk1 = 0.07;
11

12
double Setpoint1, Input1, Output1, Output1a;    // PID variables
13
PID PID1(&Input1, &Output1, &Setpoint1, Pk1, Ik1 , Dk1, DIRECT);    // PID Setup
14

15
// CAN bus baudrate. Make sure this matches for every device on the bus
16
#define CAN_BAUDRATE 250000
17

18
// ODrive node_id for odrvives
19
#define ODRV0_NODE_ID 0
20
#define ODRV1_NODE_ID 1
21
#define ODRV2_NODE_ID 2
22
#define ODRV3_NODE_ID 3
23

24
// Uncomment below the line that corresponds to your hardware.
25
// See also "Board-specific settings" to adapt the details for your hardware setup.
26

27
#define IS_TEENSY_BUILTIN // Teensy boards with built-in CAN interface (e.g. Teensy 4.1). See below to select which interface to use.
28
// #define IS_ARDUINO_BUILTIN // Arduino boards with built-in CAN interface (e.g. Arduino Uno R4 Minima)
29
// #define IS_MCP2515 // Any board with external MCP2515 based extension module. See below to configure the module.
30

31

32
/* Board-specific includes ---------------------------------------------------*/
33

34
#if defined(IS_TEENSY_BUILTIN) + defined(IS_ARDUINO_BUILTIN) + defined(IS_MCP2515) != 1
35
#warning "Select exactly one hardware option at the top of this file."
36

37
#if CAN_HOWMANY > 0 || CANFD_HOWMANY > 0
38
#define IS_ARDUINO_BUILTIN
39
#warning "guessing that this uses HardwareCAN"
40
#else
41
#error "cannot guess hardware version"
42
#endif
43

44
#endif
45

46
#ifdef IS_ARDUINO_BUILTIN
47
// See https://github.com/arduino/ArduinoCore-API/blob/master/api/HardwareCAN.h
48
// and https://github.com/arduino/ArduinoCore-renesas/tree/main/libraries/Arduino_CAN
49

50
#include <Arduino_CAN.h>
51
#include <ODriveHardwareCAN.hpp>
52
#endif // IS_ARDUINO_BUILTIN
53

54
#ifdef IS_MCP2515
55
// See https://github.com/sandeepmistry/arduino-CAN/
56
#include "MCP2515.h"
57
#include "ODriveMCPCAN.hpp"
58
#endif // IS_MCP2515
59

60
# ifdef IS_TEENSY_BUILTIN
61
// See https://github.com/tonton81/FlexCAN_T4
62
// clone https://github.com/tonton81/FlexCAN_T4.git into /src
63
#include <FlexCAN_T4.h>
64
#include "ODriveFlexCAN.hpp"
65
struct ODriveStatus; // hack to prevent teensy compile error
66
#endif // IS_TEENSY_BUILTIN
67

68
/* Board-specific settings ---------------------------------------------------*/
69

70
/* Teensy */
71

72
#ifdef IS_TEENSY_BUILTIN
73

74
FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can_intf;
75

76
bool setupCan() {
77
  can_intf.begin();
78
  can_intf.setBaudRate(CAN_BAUDRATE);
79
  can_intf.setMaxMB(16);
80
  can_intf.enableFIFO();
81
  can_intf.enableFIFOInterrupt();
82
  can_intf.onReceive(onCanMessage);
83
  return true;
84
}
85

86
#endif // IS_TEENSY_BUILTIN
87

88
/* MCP2515-based extension modules -*/
89

90
#ifdef IS_MCP2515
91

92
MCP2515Class& can_intf = CAN;
93

94
// chip select pin used for the MCP2515
95
#define MCP2515_CS 10
96

97
// interrupt pin used for the MCP2515
98
// NOTE: not all Arduino pins are interruptable, check the documentation for your board!
99
#define MCP2515_INT 2
100

101
// freqeuncy of the crystal oscillator on the MCP2515 breakout board. 
102
// common values are: 16 MHz, 12 MHz, 8 MHz
103
#define MCP2515_CLK_HZ 8000000
104

105

106
static inline void receiveCallback(int packet_size) {
107
  if (packet_size > 8) {
108
    return; // not supported
109
  }
110
  CanMsg msg = {.id = (unsigned int)CAN.packetId(), .len = (uint8_t)packet_size};
111
  CAN.readBytes(msg.buffer, packet_size);
112
  onCanMessage(msg);
113
}
114

115
bool setupCan() {
116
  // configure and initialize the CAN bus interface
117
  CAN.setPins(MCP2515_CS, MCP2515_INT);
118
  CAN.setClockFrequency(MCP2515_CLK_HZ);
119
  if (!CAN.begin(CAN_BAUDRATE)) {
120
    return false;
121
  }
122

123
  CAN.onReceive(receiveCallback);
124
  return true;
125
}
126

127
#endif // IS_MCP2515
128

129

130
/* Arduinos with built-in CAN */
131

132
#ifdef IS_ARDUINO_BUILTIN
133

134
HardwareCAN& can_intf = CAN;
135

136
bool setupCan() {
137
  return can_intf.begin((CanBitRate)CAN_BAUDRATE);
138
}
139

140
#endif
141

142
// Instantiate ODrive objects
143
ODriveCAN odrv0(wrap_can_intf(can_intf), ODRV0_NODE_ID); // Standard CAN message ID
144
ODriveCAN odrv1(wrap_can_intf(can_intf), ODRV1_NODE_ID); // Standard CAN message ID
145
ODriveCAN odrv2(wrap_can_intf(can_intf), ODRV2_NODE_ID); // Standard CAN message ID
146
ODriveCAN odrv3(wrap_can_intf(can_intf), ODRV3_NODE_ID); // Standard CAN message ID
147
ODriveCAN* odrives[] = {&odrv0, &odrv1, &odrv2, &odrv3}; // Make sure all ODriveCAN instances are accounted for here
148

149
struct ODriveUserData0 {
150
  Heartbeat_msg_t last_heartbeat;
151
  bool received_heartbeat = false;
152
  Get_Encoder_Estimates_msg_t last_feedback;
153
  bool received_feedback = false;
154
};
155

156
struct ODriveUserData1 {
157
  Heartbeat_msg_t last_heartbeat;
158
  bool received_heartbeat = false;
159
  Get_Encoder_Estimates_msg_t last_feedback;
160
  bool received_feedback = false;
161
};
162

163
struct ODriveUserData2 {
164
  Heartbeat_msg_t last_heartbeat;
165
  bool received_heartbeat = false;
166
  Get_Encoder_Estimates_msg_t last_feedback;
167
  bool received_feedback = false;
168
};
169

170
struct ODriveUserData3 {
171
  Heartbeat_msg_t last_heartbeat;
172
  bool received_heartbeat = false;
173
  Get_Encoder_Estimates_msg_t last_feedback;
174
  bool received_feedback = false;
175
};
176

177
// Keep some application-specific user data for every ODrive.
178
ODriveUserData0 odrv0_user_data;
179
ODriveUserData1 odrv1_user_data;
180
ODriveUserData2 odrv2_user_data;
181
ODriveUserData3 odrv3_user_data;
182

183
// Called every time a Heartbeat message arrives from the ODrive
184
void onHeartbeat(Heartbeat_msg_t& msg, void* user_data) {
185
  
186
  ODriveUserData0* odrv_user_data0 = static_cast<ODriveUserData0*>(user_data);
187
  odrv_user_data0->last_heartbeat = msg;
188
  odrv_user_data0->received_heartbeat = true;
189

190
  ODriveUserData1* odrv_user_data1 = static_cast<ODriveUserData1*>(user_data);
191
  odrv_user_data1->last_heartbeat = msg;
192
  odrv_user_data1->received_heartbeat = true;
193

194
  ODriveUserData2* odrv_user_data2 = static_cast<ODriveUserData2*>(user_data);
195
  odrv_user_data2->last_heartbeat = msg;
196
  odrv_user_data2->received_heartbeat = true;
197

198
  ODriveUserData3* odrv_user_data3 = static_cast<ODriveUserData3*>(user_data);
199
  odrv_user_data3->last_heartbeat = msg;
200
  odrv_user_data3->received_heartbeat = true;
201

202

203
}
204

205
// Called every time a feedback message arrives from the ODrive
206
void onFeedback(Get_Encoder_Estimates_msg_t& msg, void* user_data) {
207
  ODriveUserData0* odrv_user_data0 = static_cast<ODriveUserData0*>(user_data);
208
  odrv_user_data0->last_feedback = msg;
209
  odrv_user_data0->received_feedback = true;
210

211
  ODriveUserData1* odrv_user_data1 = static_cast<ODriveUserData1*>(user_data);
212
  odrv_user_data1->last_feedback = msg;
213
  odrv_user_data1->received_feedback = true;
214

215
  ODriveUserData2* odrv_user_data2 = static_cast<ODriveUserData2*>(user_data);
216
  odrv_user_data2->last_feedback = msg;
217
  odrv_user_data2->received_feedback = true;
218

219
  ODriveUserData3* odrv_user_data3 = static_cast<ODriveUserData3*>(user_data);
220
  odrv_user_data3->last_feedback = msg;
221
  odrv_user_data3->received_feedback = true;
222
}
223

224
// Called for every message that arrives on the CAN bus
225
void onCanMessage(const CanMsg& msg) {
226
  for (auto odrive: odrives) {
227
    onReceive(msg, *odrive);
228
  }
229

230
}
231

232
// Uncomment below the line that corresponds to your hardware.
233
// See also "Board-specific settings" to adapt the details for your hardware setup.
234

235
#define IS_TEENSY_BUILTIN // Teensy boards with built-in CAN interface (e.g. Teensy 4.1). See below to select which interface to use.
236
// #define IS_ARDUINO_BUILTIN // Arduino boards with built-in CAN interface (e.g. Arduino Uno R4 Minima)
237
// #define IS_MCP2515 // Any board with external MCP2515 based extension module. See below to configure the module.
238

239
#include <Wire.h>
240

241
// #include <FastGPIO.h>
242
// #define APA102_USE_FAST_GPIO
243

244
#include <APA102.h>
245
// Define which pins to use.
246
const uint8_t dataPin = 2;
247
const uint8_t clockPin = 3;
248

249
// ** DEFINE VARIABLES **
250

251
int LED0;
252
int LED1;
253
int LED2;
254
int LED3;
255
int LED4;
256
int LED5;
257
int LED6;
258

259
float roll;
260
float rollTrim;
261
float rollTrimmed;
262

263
float fowardVel;
264
float fowardVelFiltered;
265

266
float rotVel;
267
float rotVelFiltered;
268

269
float wheel1;
270
float wheel2;
271
float wheel3;
272
float wheel4;
273

274
int pot1;   // ctrl panel top pot
275
float pot2;   // ctrl panel bottom pot
276
int pot3;   // twist grip RH
277
int pot4;   // twist grip LH
278
int sw1;    // ctrl panel switch
279
int sw2;    // handlebar switch RH
280
int sw3;    // handlebar switch LH
281

282
int clFlag;  // ODrive init flag
283

284
int EMflag = 0;  // Emergency stop flat - if it falls over
285

286
unsigned long currentMillis;
287
unsigned long previousMillis = 0;        // set up timers
288
long interval = 10;             // time constant for timer
289

290
// Create an object for writing to the LED strip.
291
APA102<dataPin, clockPin> ledStrip;
292

293
// Set the number of LEDs to control.
294
const uint16_t ledCount = 14;
295

296
#include "SparkFun_BNO08x_Arduino_Library.h"  // CTRL+Click here to get the library: http://librarymanager/All#SparkFun_BNO08x
297
BNO08x myIMU;
298

299
#define BNO08X_INT  17
300
#define BNO08X_RST  16
301
#define BNO08X_ADDR 0x4B  // SparkFun BNO08x Breakout (Qwiic) defaults to 0x4B
302

303
void setup() {
304

305
  pinMode(A10, INPUT);    // ctrl panel top pot
306
  pinMode(A11, INPUT);    // ctrl panel bottom pot
307
  pinMode(A12, INPUT);    // external pot blue wire
308
  pinMode(A13, INPUT);    // external pot white wire
309

310
  pinMode(4, INPUT_PULLUP);   // ctrl panel switch
311
  pinMode(5, INPUT_PULLUP);   // external switch yellow wire
312
  pinMode(6, INPUT_PULLUP);   // external switch green wire
313
  
314
  Serial.begin(115200);
315
  Serial.println();
316
  Serial.println("BNO08x Read Example");
317

318
  PID1.SetMode(AUTOMATIC);              
319
  PID1.SetOutputLimits(-80, 80);
320
  PID1.SetSampleTime(10);
321

322
  Wire.begin();
323

324
  if (myIMU.begin(BNO08X_ADDR, Wire, BNO08X_INT, BNO08X_RST) == false) {
325
    Serial.println("BNO08x not detected at default I2C address. Check your jumpers and the hookup guide. Freezing...");
326
    ledStrip.startFrame();
327
    ledStrip.sendColor(LED0, LED0, LED0);
328
    ledStrip.sendColor(LED1, LED1, LED1);
329
    ledStrip.sendColor(LED2, LED2, LED2);  
330
    ledStrip.sendColor(50, LED3, LED3);       // error LED
331
    ledStrip.sendColor(LED4, LED4, LED4);   
332
    ledStrip.sendColor(LED5, LED5, LED5);   
333
    ledStrip.sendColor(LED6, LED6, LED6);  
334
    ledStrip.endFrame(7);
335
    while (1)
336
      ;
337
  }
338
  
339
  Serial.println("BNO08x found!");
340
  ledStrip.startFrame();
341
  ledStrip.sendColor(LED0, LED0, LED0);
342
  ledStrip.sendColor(LED1, LED1, LED1);
343
  ledStrip.sendColor(LED2, LED2, LED2);  
344
  ledStrip.sendColor(LED1, 50, LED3);       // no error LED
345
  ledStrip.sendColor(LED4, LED4, LED4);   
346
  ledStrip.sendColor(LED5, LED5, LED5);   
347
  ledStrip.sendColor(LED6, LED6, LED6);  
348
  ledStrip.endFrame(7);
349

350
  setReports();
351

352
  Serial.println("Reading events");
353
  delay(100);
354

355
  // Wait for up to 3 seconds for the serial port to be opened on the PC side.
356
  // If no PC connects, continue anyway.
357
  for (int i = 0; i < 30 && !Serial; ++i) {
358
    delay(100);
359
  }
360
  delay(200);
361

362
  Serial.println("Starting ODriveCAN");
363

364
  // Register callbacks for the heartbeat and encoder feedback messages
365
  odrv0.onFeedback(onFeedback, &odrv0_user_data);
366
  odrv0.onStatus(onHeartbeat, &odrv0_user_data);
367
  odrv1.onFeedback(onFeedback, &odrv1_user_data);
368
  odrv1.onStatus(onHeartbeat, &odrv1_user_data);
369
  odrv2.onFeedback(onFeedback, &odrv2_user_data);
370
  odrv2.onStatus(onHeartbeat, &odrv2_user_data);
371
  odrv3.onFeedback(onFeedback, &odrv3_user_data);
372
  odrv3.onStatus(onHeartbeat, &odrv3_user_data);
373

374
  // Configure and initialize the CAN bus interface. This function depends on
375
  // your hardware and the CAN stack that you're using.
376
  if (!setupCan()) {
377
    Serial.println("CAN failed to initialize: reset required");
378
    while (true); // spin indefinitely
379
  }
380

381
    // Check for Odrives
382
    Serial.println("Waiting for ODrive 0...");
383
    while (!odrv0_user_data.received_heartbeat) {
384
      pumpEvents(can_intf);
385
      delay(100);
386
    }
387
    Serial.println("found ODrive 0");
388
    
389
    Serial.println("Waiting for ODrive 1...");
390
    while (!odrv1_user_data.received_heartbeat) {
391
      pumpEvents(can_intf);
392
      delay(100);
393
    }
394
    Serial.println("found ODrive 1");
395
   
396
    Serial.println("Waiting for ODrive 2...");
397
    while (!odrv2_user_data.received_heartbeat) {
398
      pumpEvents(can_intf);
399
      delay(100);
400
    }
401
    Serial.println("found ODrive 2");
402
 
403
    Serial.println("Waiting for ODrive 3...");
404
    while (!odrv3_user_data.received_heartbeat) {
405
      pumpEvents(can_intf);
406
      delay(100);
407
    }
408
    Serial.println("found ODrive 3");
409
  
410
    odrv0.setControllerMode(CONTROL_MODE_VELOCITY_CONTROL, INPUT_MODE_PASSTHROUGH);
411
    odrv1.setControllerMode(CONTROL_MODE_VELOCITY_CONTROL, INPUT_MODE_PASSTHROUGH);
412
    odrv2.setControllerMode(CONTROL_MODE_VELOCITY_CONTROL, INPUT_MODE_PASSTHROUGH);
413
    odrv3.setControllerMode(CONTROL_MODE_VELOCITY_CONTROL, INPUT_MODE_PASSTHROUGH);
414

415
    for (int i = 0; i < 15; ++i) {
416
      delay(10);
417
      pumpEvents(can_intf);
418
    }
419
}
420

421
// Here is where you define the sensor outputs you want to receive
422
void setReports(void) {
423
  Serial.println("Setting desired reports");
424
  if (myIMU.enableRotationVector() == true) {
425
    Serial.println(F("Rotation vector enabled"));
426
    Serial.println(F("Output in form roll, pitch, yaw"));
427
  } else {
428
    Serial.println("Could not enable rotation vector");
429
  }
430
}
431

432
// motion filter to filter motions
433

434
float filter(float prevValue, float currentValue, int filter) {  
435
  float lengthFiltered =  (prevValue + (currentValue * filter)) / (filter + 1);  
436
  return lengthFiltered;  
437
}
438

439
void loop() {
440

441
      // ** READ IMU DATA **
442
      if (myIMU.wasReset()) {
443
        Serial.println("sensor was reset ");
444
        setReports();
445
      }    
446
      // Has a new event come in on the Sensor Hub Bus?
447
      if (myIMU.getSensorEvent() == true) {    
448
        // is it the correct sensor data we want?
449
        if (myIMU.getSensorEventID() == SENSOR_REPORTID_ROTATION_VECTOR) {     
450
            roll = (myIMU.getRoll()) * 180.0 / PI; // Convert roll to degrees
451
        }
452
      }
453
      // ** END OF READ IMU DATA **
454

455
      // ** CAN / ODRIVE STUFF **
456
      pumpEvents(can_intf); // This is required on some platforms to handle incoming feedback CAN messages
457

458

459
      // ** TIMED LOOP AT 10MS FOR EVERTHING ELSE ** 
460
      currentMillis = millis();
461
          if (currentMillis - previousMillis >= 10) {
462
          previousMillis = currentMillis;
463

464
          // ** READ SWITCHES **
465
          pot1 = analogRead(A10);       // top pot
466
          pot2 = analogRead(A11);       // bottom pot
467
          pot3 = analogRead(A13);       // left twist
468
          pot4 = analogRead(A12);       // right twist
469
          sw1 = digitalRead(4);         // ctrl panel switch
470
          sw2 = digitalRead(6);         // left handlebar switch
471
          sw3 = digitalRead(5);         // right handlebar switch
472

473
          if (sw1 == 0) {          // Init Odrives
474
              Serial.println("Enabling closed loop control 0...");
475
                while (odrv0_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) {
476
                odrv0.clearErrors();
477
                delay(1);
478
                odrv0.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
479
              }
480
            
481
              Serial.println("Enabling closed loop control 1...");
482
              while (odrv1_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) {
483
                odrv1.clearErrors();
484
                delay(1);
485
                odrv1.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
486
              }
487
            
488
              Serial.println("Enabling closed loop control 2...");
489
              while (odrv2_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) {
490
                odrv2.clearErrors();
491
                delay(1);
492
                odrv2.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
493
              }
494
           
495
              Serial.println("Enabling closed loop control 3...");
496
              while (odrv3_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) {
497
                odrv3.clearErrors();
498
                delay(1);
499
                odrv3.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
500
              } 
501
              clFlag = 1;          // reset flag
502
          }
503

504
          else if (sw1 == 1) {      // make sure we only power on ODrives once per button press
505
            clFlag = 0;
506
          }
507

508
          rollTrim = (float) (pot1 - 512)/100;
509
          rollTrimmed = (roll + rollTrim);
510
    
511
          // display IMU data on LEDs
512
      
513
          if (rollTrimmed > -1.5 && rollTrimmed < 1.5) {
514
            LED0 = 0;
515
            LED1 = 0;
516
            LED2 = 0;
517
            LED3 = 50;
518
            LED4 = 0;
519
            LED5 = 0;
520
            LED6 = 0;      
521
          }
522
          else if (rollTrimmed > -3 && rollTrimmed < -1.5) {
523
            LED0 = 0;
524
            LED1 = 0;
525
            LED2 = 50;
526
            LED3 = 0;
527
            LED4 = 0;
528
            LED5 = 0;
529
            LED6 = 0;     
530
          }
531
          else if (rollTrimmed < 3 && rollTrimmed > 1.5) {
532
            LED0 = 0;
533
            LED1 = 0;
534
            LED2 = 0;
535
            LED3 = 0;
536
            LED4 = 50;
537
            LED5 = 0;
538
            LED6 = 0;      
539
          }
540
          else if (rollTrimmed > -4.5 && rollTrimmed < -3) {
541
            LED0 = 0;
542
            LED1 = 50;
543
            LED2 = 0;
544
            LED3 = 0;
545
            LED4 = 0;
546
            LED5 = 0;
547
            LED6 = 0;      
548
          }
549
          else if (rollTrimmed < -4.5) {
550
            LED0 = 50;
551
            LED1 = 0;
552
            LED2 = 0;
553
            LED3 = 0;
554
            LED4 = 0;
555
            LED5 = 0;
556
            LED6 = 0;      
557
          }
558
          else if (rollTrimmed < 4.5 && rollTrimmed > 3) {
559
            LED0 = 0;
560
            LED1 = 0;
561
            LED2 = 0;
562
            LED3 = 0;
563
            LED4 = 0;
564
            LED5 = 50;
565
            LED6 = 0;      
566
          }
567
          else if (rollTrimmed > 4.5) {
568
            LED0 = 0;
569
            LED1 = 0;
570
            LED2 = 0;
571
            LED3 = 0;
572
            LED4 = 0;
573
            LED5 = 0;
574
            LED6 = 50;      
575
          }
576
          else {
577
            LED0 = 0;
578
            LED1 = 0;
579
            LED2 = 0;
580
            LED3 = 0;
581
            LED4 = 0;
582
            LED5 = 0;
583
            LED6 = 0;
584
          }
585
        
586
          ledStrip.startFrame();                          // Write to LEDs
587
          ledStrip.sendColor(LED0, LED0, LED0);
588
          ledStrip.sendColor(LED1, LED1, LED1);
589
          ledStrip.sendColor(LED2, LED2, LED2);  
590
          ledStrip.sendColor(LED3, LED3, LED3);   
591
          ledStrip.sendColor(LED4, LED4, LED4);   
592
          ledStrip.sendColor(LED5, LED5, LED5);   
593
          ledStrip.sendColor(LED6, LED6, LED6);   
594
          ledStrip.endFrame(7); 
595

596
          Input1 = rollTrimmed;                     // use ctrl panel pot for IMU trim
597
          Setpoint1 = 0;
598
          PID1.Compute();
599

600
          pot2 = pot2 / 1000;                       // ctrl panel pot for manual gain control         
601
          pot2 = constrain(pot2,0,1);
602
          Output1a = Output1*pot2;
603
          
604
          fowardVel = (float) (pot3 - 300);         // RH twist grip for going forwards/backwards
605
          fowardVel = constrain(fowardVel,0,600);
606
          fowardVel = fowardVel/25;
607

608
          if (sw2 == 0) {                           // go forwards or backwards
609
             fowardVel = fowardVel  *-1;
610
          }
611

612
          rotVel = (float) (pot4 - 300);            // LH twist grip for going forwards/backwards
613
          rotVel = constrain(rotVel,0,600);
614
          rotVel = rotVel/40;
615

616
          fowardVelFiltered = filter(fowardVel, fowardVelFiltered,20);
617

618
          if (sw3 == 0) {                           // go forwards or backwards
619
             rotVel = rotVel  *-1;
620
          }
621

622
          rotVelFiltered = filter(rotVel, rotVelFiltered,20);
623

624
          if (rollTrimmed < -10 || rollTrimmed > 10) {
625
            EMflag = 1;
626
          }
627

628
          wheel1 = Output1a + fowardVelFiltered - rotVelFiltered;                        // 1 front wheel
629
          wheel2 = Output1a - fowardVelFiltered - (rotVelFiltered/2.176);                // 2 front mid wheel
630
          wheel3 = Output1a + fowardVelFiltered + (rotVelFiltered/2.176);                // 3 back mid wheel
631
          wheel4 = Output1a - fowardVelFiltered + rotVelFiltered;                        // 4 back wheel
632

633
          // ** DRIVE WHEELS UNDER NORMAL CIRUMSTANCES **
634
          if (EMflag == 0) {                                                              
635
              odrv2.setVelocity(wheel1);        // 1 front wheel
636
              odrv1.setVelocity(wheel2*-1);     // 2 front mid wheel
637
              odrv3.setVelocity(wheel3);        // 3 back mid wheel
638
              odrv0.setVelocity(wheel4*-1);     // 4 back wheel
639
          }
640

641
          // ** STOP WHEELS UNTIL EVERYTHING IS RESET **
642
          else if (EMflag == 1) {
643
              odrv2.setVelocity(0);           // 1 front wheel
644
              odrv1.setVelocity(0);           // 2 front mid wheel
645
              odrv3.setVelocity(0);           // 3 back mid wheel
646
              odrv0.setVelocity(0);           // 4 back wheel            
647
          }
648

649
          Serial.println(EMflag);
650
      
651
  } // ** END OF TIMED LOOP **
652
  
653
} // end of main loop
654
screwbike

Использование cookies
