Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / main.cpp @ 9c46b728

History | View | Annotate | Download (50.4 KB)

1
#define BL_CALLBACK_TABLE_ADDR  (0x08000000 + 0x01C0)
2
#define BL_MAGIC_NUMBER         ((uint32_t)0xFF669900u)
3

    
4
#define SHUTDOWN_NONE             0
5
#define SHUTDOWN_TRANSPORTATION   1
6
#define SHUTDOWN_DEEPSLEEP        2
7
#define SHUTDOWN_HIBERNATE        3
8
#define SHUTDOWN_RESTART          4
9
#define SHUTDOWN_HANDLE_REQUEST   5
10

    
11
#include <ch.hpp>
12

    
13
#include <amiro/util/util.h>
14
#include <global.hpp>
15
#include <exti.hpp>
16

    
17
#include <chprintf.h>
18
#include <shell.h>
19

    
20
#include "linefollow2.hpp" 
21

    
22
using namespace chibios_rt;
23

    
24
Global global;
25

    
26
struct blVersion_t {
27
  const uint8_t identifier;
28
  const uint8_t major;
29
  const uint8_t minor;
30
  const uint8_t patch;
31
} __attribute__((packed));
32

    
33
void systemShutdown() {
34
  types::kinematic k;
35
  uint8_t i;
36

    
37
//  // make sure we assert SYS_PD_N to delay shutdown until we're done.
38
//  boardRequestShutdown();
39

    
40
    // stop the user thread
41
  global.userThread.requestTerminate();
42
  global.userThread.wait();
43

    
44
  k.x = 0x00u;
45
  k.w_z = 0x00u;
46

    
47
  // stop wheels
48
  global.robot.setTargetSpeed(k);
49
  global.robot.terminate();
50

    
51
  for (i = 0x00; i < global.vcnl4020.size(); i++) {
52
    global.vcnl4020[i].requestTerminate();
53
    global.vcnl4020[i].wait();
54
  }
55

    
56
  global.ina219.requestTerminate();
57
  global.ina219.wait();
58
  global.hmc5883l.requestTerminate();
59
  global.hmc5883l.wait();
60
  global.l3g4200d.requestTerminate();
61
  global.l3g4200d.wait();
62

    
63
  global.motorcontrol.requestTerminate();
64
  global.motorcontrol.wait();
65
  global.odometry.requestTerminate();
66
  global.odometry.wait();
67

    
68
  // stop I²C
69
  for (i = 0; i < global.V_I2C2.size(); ++i)
70
    global.V_I2C2[i].stop();
71
  global.HW_I2C2.stop();
72

    
73
  global.lis331dlh.requestTerminate();
74
  global.lis331dlh.wait();
75

    
76
  global.lis331dlh.configure(&global.accel_sleep_config);
77
//  global.lis331dlh.start(NORMALPRIO +4);
78

    
79
//  boardWriteIoPower(0);
80
//  boardStandby();
81

    
82
  return;
83
}
84

    
85

    
86
//void (*shellcmd_t)(BaseSequentialStream *chp, int argc, char *argv[]);
87

    
88
void shellRequestShutdown(BaseSequentialStream *chp, int argc, char *argv[]) {
89

    
90
  chprintf(chp, "shellRequestShutdown\n");
91

    
92
  /* if nor argument was given, print some help text */
93
  if (argc == 0 || strcmp(argv[0], "help") == 0) {
94
    chprintf(chp, "\tUSAGE:\n");
95
    chprintf(chp, "> shutdown <type>\n");
96
    chprintf(chp, "\n");
97
    chprintf(chp, "\ttype\n");
98
    chprintf(chp, "The type of shutdown to perform.\n");
99
    chprintf(chp, "Choose one of the following types:\n");
100
    chprintf(chp, "  transportation - Ultra low-power mode with all wakeups disabled.\n");
101
    chprintf(chp, "                   The robot can not be charged.\n");
102
    chprintf(chp, "  deepsleep      - Ultra low-power mode with several wakeups enabled.\n");
103
    chprintf(chp, "                   The robot can only be charged via the power plug.\n");
104
    chprintf(chp, "  hibernate      - Medium low-power mode, but with full charging capabilities.\n");
105
    chprintf(chp, "  restart        - Performs a system restart.\n");
106
    chprintf(chp, "Alternatively, you can use the shortcuts 't', 'd', 'h', and 'r' respectively.");
107
    chprintf(chp, "\n");
108
    return;
109
  }
110

    
111
  if (strcmp(argv[0],"transportation") == 0 || strcmp(argv[0],"t") == 0) {
112
    shutdown_now = SHUTDOWN_TRANSPORTATION;
113
    chprintf(chp, "shutdown to transportation mode initialized\n");
114
  } else if (strcmp(argv[0],"deepsleep") == 0 || strcmp(argv[0],"d") == 0) {
115
    shutdown_now = SHUTDOWN_DEEPSLEEP;
116
    chprintf(chp, "shutdown to deepsleep mode initialized\n");
117
  } else if (strcmp(argv[0],"hibernate") == 0 || strcmp(argv[0],"h") == 0) {
118
    shutdown_now = SHUTDOWN_HIBERNATE;
119
    chprintf(chp, "shutdown to hibernate mode initialized\n");
120
  } else if (strcmp(argv[0],"restart") == 0 || strcmp(argv[0],"r") == 0) {
121
    chprintf(chp, "restart initialized\n");
122
    shutdown_now = SHUTDOWN_RESTART;
123
  } else {
124
    chprintf(chp, "ERROR: unknown argument!\n");
125
    shutdown_now = SHUTDOWN_NONE;
126
  }
127

    
128
  return;
129
}
130

    
131
void shellRequestWakeup(BaseSequentialStream *chp, int argc, char *argv[]) {
132
  int i;
133
  chprintf(chp, "shellRequestWakeup\n");
134

    
135
  for (i = 0x00u; i < argc; i++)
136
    chprintf(chp, "%s\n", argv[i]);
137

    
138
  boardWakeup();
139
}
140

    
141
void shellRequestGetMemoryData(BaseSequentialStream *chp, int argc, char *argv[]) {
142
  enum Type {HEX, U8, U16, U32, S8, S16, S32};
143

    
144
  chprintf(chp, "shellRequestReadData\n");
145

    
146
  if (argc < 2 || strcmp(argv[0],"help") == 0)
147
  {
148
    chprintf(chp, "Usage: %s\n","get_memory_data <type> <start> [<count>]");
149
    chprintf(chp, "\n");
150
    chprintf(chp, "\ttype\n");
151
    chprintf(chp, "The data type as which to interpret the data.\n");
152
    chprintf(chp, "Choose one of the following types:\n");
153
    chprintf(chp, "  hex - one byte as hexadecimal value\n");
154
    chprintf(chp, "  u8  - unsigned integer (8 bit)\n");
155
    chprintf(chp, "  u16 - unsigned integer (16 bit)\n");
156
    chprintf(chp, "  u32 - unsigned integer (32 bit)\n");
157
    chprintf(chp, "  s8  - signed integer (8 bit)\n");
158
    chprintf(chp, "  s16 - signed integer (16 bit)\n");
159
    chprintf(chp, "  s32 - signed integer (32 bit)\n");
160
    chprintf(chp, "\tstart\n");
161
    chprintf(chp, "The first byte to read from the memory.\n");
162
    chprintf(chp, "\tcount [default = 1]\n");
163
    chprintf(chp, "The number of elements to read.\n");
164
    chprintf(chp, "\n");
165
    chprintf(chp, "\tNOTE\n");
166
    chprintf(chp, "Type conversions of this function might fail.\n");
167
    chprintf(chp, "If so, use type=hex and convert by hand.\n");
168
    chprintf(chp, "\n");
169
    return;
170
  }
171

    
172
  uint8_t type_size = 0;
173
  Type type = HEX;
174
  if (strcmp(argv[0],"hex") == 0) {
175
    type_size = sizeof(unsigned char);
176
    type = HEX;
177
  } else if(strcmp(argv[0],"u8") == 0) {
178
    type_size = sizeof(uint8_t);
179
    type = U8;
180
  } else if(strcmp(argv[0],"u16") == 0) {
181
    type_size = sizeof(uint16_t);
182
    type = U16;
183
  } else if(strcmp(argv[0],"u32") == 0) {
184
    type_size = sizeof(uint32_t);
185
    type = U32;
186
  } else if(strcmp(argv[0],"s8") == 0) {
187
    type_size = sizeof(int8_t);
188
    type = S8;
189
  } else if(strcmp(argv[0],"s16") == 0) {
190
    type_size = sizeof(int16_t);
191
    type = S16;
192
  } else if(strcmp(argv[0],"s32") == 0) {
193
    type_size = sizeof(int32_t);
194
    type = S32;
195
  } else {
196
    chprintf(chp, "First argument invalid. Use 'get_memory_data help' for help.\n");
197
    return;
198
  }
199

    
200
  unsigned int start_byte = atoi(argv[1]);
201

    
202
  unsigned int num_elements = 1;
203
  if (argc >= 3)
204
    num_elements = atoi(argv[2]);
205

    
206
  const size_t eeprom_size = EEPROM::getsize(&global.at24c01);
207
  uint8_t buffer[eeprom_size];
208
  if (start_byte + (type_size * num_elements) > eeprom_size) {
209
    num_elements = (eeprom_size - start_byte) / type_size;
210
    chprintf(chp, "Warning: request exceeds eeprom size -> limiting to %u values.\n", num_elements);
211
  }
212

    
213
  chFileStreamSeek((BaseFileStream*)&global.at24c01, start_byte);
214

    
215
  // Work around, because stm32f1 cannot read a single byte
216
  if (type_size*num_elements < 2)
217
    type_size = 2;
218

    
219
  uint32_t bytes_read = chSequentialStreamRead((BaseFileStream*)&global.at24c01, buffer, type_size*num_elements);
220

    
221
  if (bytes_read != type_size*num_elements)
222
    chprintf(chp, "Warning: %u of %u requested bytes were read.\n", bytes_read, type_size*num_elements);
223

    
224
  for (unsigned int i = 0; i < num_elements; ++i) {
225
    switch (type) {
226
      case HEX:
227
        chprintf(chp, "%02X ", buffer[i]);
228
        break;
229
      case U8:
230
        chprintf(chp, "%03u ", ((uint8_t*)buffer)[i]);
231
        break;
232
      case U16:
233
        chprintf(chp, "%05u ", ((uint16_t*)buffer)[i]);
234
        break;
235
      case U32:
236
        chprintf(chp, "%010u ", ((uint32_t*)buffer)[i]);
237
        break;
238
      case S8:
239
        chprintf(chp, "%+03d ", ((int8_t*)buffer)[i]);
240
        break;
241
      case S16:
242
        chprintf(chp, "%+05d ", ((int16_t*)buffer)[i]);
243
        break;
244
      case S32:
245
        chprintf(chp, "%+010d ", ((int32_t*)buffer)[i]);
246
        break;
247
      default:
248
        break;
249
    }
250
  }
251
  chprintf(chp, "\n");
252

    
253
  return;
254
}
255

    
256
void shellRequestSetLights(BaseSequentialStream *chp, int argc, char *argv[]) {
257

    
258
  if (argc < 2 || argc == 3 ||strcmp(argv[0],"help") == 0) {
259
    chprintf(chp, "\tUSAGE:\n");
260
    chprintf(chp, "> set_lights <led mask> <white/red> [<green> <blue>]\n");
261
    chprintf(chp, "\n");
262
    chprintf(chp, "\tled mask\n");
263
    chprintf(chp, "The LEDs to be set.\n");
264
    chprintf(chp, "You can set multiple LEDs at once by adding the following values:\n");
265
    chprintf(chp, "  0x01 - rear left LED (SSW)\n");
266
    chprintf(chp, "  0x02 - left rear LED (WSW)\n");
267
    chprintf(chp, "  0x04 - left front LED (WNW)\n");
268
    chprintf(chp, "  0x08 - front left LED (NNW)\n");
269
    chprintf(chp, "  0x10 - front right LED (NNE)\n");
270
    chprintf(chp, "  0x20 - right front LED (ENE)\n");
271
    chprintf(chp, "  0x40 - right rear LED (ESE)\n");
272
    chprintf(chp, "  0x80 - rear right LED (SSE)\n");
273
    chprintf(chp, "\twhite/red\n");
274
    chprintf(chp, "If no optional argument is given, this arguments sets the white value of the selected LEDs.\n");
275
    chprintf(chp, "Otherwise this arguments sets the red color channel value.\n");
276
    chprintf(chp, "\tgreen\n");
277
    chprintf(chp, "Sets the green color channel value.\n");
278
    chprintf(chp, "\tblue\n");
279
    chprintf(chp, "Sets the blue color channel value.\n");
280
    chprintf(chp, "\n");
281
    chprintf(chp, "\tExample:\n");
282
    chprintf(chp, "This line will set the two most left and two most right LEDs to bright cyan.\n");
283
    chprintf(chp, "> set_lights 0x66 0 255 255\n");
284
    chprintf(chp, "\n");
285
    return;
286
  }
287

    
288
  int arg_mask = strtol(argv[0], NULL, 0);
289
  int red = strtol(argv[1], NULL, 0);
290
  int green = red;
291
  int blue = red;
292
  if (argc >= 4) {
293
    green = strtol(argv[2], NULL, 0);
294
    blue = strtol(argv[3], NULL, 0);
295
  }
296
  Color color(red, green, blue);
297

    
298
  if (arg_mask & 0x01) {
299
    global.robot.setLightColor(constants::LightRing::LED_SSW, color);
300
  }
301
  if (arg_mask & 0x02) {
302
    global.robot.setLightColor(constants::LightRing::LED_WSW, color);
303
  }
304
  if (arg_mask & 0x04) {
305
    global.robot.setLightColor(constants::LightRing::LED_WNW, color);
306
  }
307
  if (arg_mask & 0x08) {
308
    global.robot.setLightColor(constants::LightRing::LED_NNW, color);
309
  }
310
  if (arg_mask & 0x10) {
311
    global.robot.setLightColor(constants::LightRing::LED_NNE, color);
312
  }
313
  if (arg_mask & 0x20) {
314
    global.robot.setLightColor(constants::LightRing::LED_ENE, color);
315
  }
316
  if (arg_mask & 0x40) {
317
    global.robot.setLightColor(constants::LightRing::LED_ESE, color);
318
  }
319
  if (arg_mask & 0x80) {
320
    global.robot.setLightColor(constants::LightRing::LED_SSE, color);
321
  }
322

    
323
  return;
324
}
325

    
326
void boardPeripheryCheck(BaseSequentialStream *chp) {
327
  msg_t result;
328
  chprintf(chp, "\nCHECK: START\n");
329
  // Check the accelerometer
330
  result = global.lis331dlh.getCheck();
331
  if (result == global.lis331dlh.CHECK_OK)
332
    chprintf(chp, "LIS331DLH: OK\n");
333
  else
334
    chprintf(chp, "LIS331DLH: FAIL\n");
335

    
336
  // Self-test accelerometer
337
//  lis331dlh.printSelfTest(NULL);
338

    
339
  // Check the eeprom
340
  result = global.memory.getCheck();
341
  if ( result != global.memory.OK)
342
    chprintf(chp, "Memory Structure: FAIL\n");
343
  else
344
    chprintf(chp, "Memory Structure: OK\n");
345

    
346
  // Check the gyroscope
347
  result = global.l3g4200d.getCheck();
348
  if (result == global.l3g4200d.CHECK_OK)
349
    chprintf(chp, "L3G4200D: OK\n");
350
  else
351
    chprintf(chp, "L3G4200D: FAIL\n");
352

    
353
  // Check the magnetometer
354
  result = global.hmc5883l.getCheck();
355
  if (result == global.hmc5883l.CHECK_OK)
356
    chprintf(chp, "HMC5883L: OK\n");
357
  else
358
    chprintf(chp, "HMC5883L: FAIL\n");
359

    
360
  // Check the MUX
361
  result = global.HW_PCA9544.getCheck();
362
  if (result == global.HW_PCA9544.CHECK_OK)
363
    chprintf(chp, "PCA9544: OK\n");
364
  else
365
    chprintf(chp, "PCA9544: FAIL\n");
366

    
367
  // Check the power monitor
368
  chprintf(chp, "INA219:\tVDD (3.3V):\n");
369
  result = global.ina219.selftest();
370
  if (result == BaseSensor<>::NOT_IMPLEMENTED)
371
    chprintf(chp, "->\tnot implemented\n");
372
  else if (result != INA219::Driver::ST_OK)
373
    chprintf(chp, "->\tFAIL (error code 0x%02X)\n", result);
374
  else
375
    chprintf(chp, "->\tOK\n");
376

    
377
  // Check the proximitysensors
378
  for (uint8_t i = 0x00; i < global.vcnl4020.size(); i++) {
379
    result = global.vcnl4020[i].getCheck();
380
    if (result == global.vcnl4020[i].CHECK_OK)
381
      chprintf(chp, "VCNL4020: %d OK\n", i);
382
    else
383
      chprintf(chp, "VCNL4020: %d FAIL\n", i);
384
  }
385
  chprintf(chp, "CHECK: FINISH\n");
386
}
387

    
388
void shellRequestCheck(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
389
  chprintf(chp, "shellRequestCheck\n");
390
  boardPeripheryCheck(chp);
391
}
392

    
393
void shellRequestResetMemory(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
394
  chprintf(chp, "shellRequestInitMemory\n");
395

    
396
  msg_t res = global.memory.resetMemory();
397
  if ( res != global.memory.OK)
398
    chprintf(chp, "Memory Init: FAIL\n");
399
  else
400
    chprintf(chp, "Memory Init: OK\n");
401
}
402

    
403
void shellRequestGetBoardId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
404
  chprintf(chp, "shellRequestGetBoardId\n");
405
  uint8_t id = 0xFFu;
406

    
407
  msg_t res = global.memory.getBoardId(&id);
408

    
409
  if (res != global.memory.OK)
410
    chprintf(chp, "Get Board ID: FAIL\n");
411
  else
412
    chprintf(chp, "Get Board ID: %u\n", id);
413
}
414

    
415
void shellRequestSetBoardId(BaseSequentialStream *chp, int argc, char *argv[]) {
416
  chprintf(chp, "shellRequestSetBoardId\n");
417

    
418
  if (argc == 0) {
419
    chprintf(chp, "Usage: %s\n","set_board_id <idx>");
420
  } else {
421
    msg_t res = global.memory.setBoardId(atoi(argv[0]));
422
    if (res != global.memory.OK)
423
      chprintf(chp, "Set Board ID: FAIL\n");
424
    else
425
      chprintf(chp, "Set Board ID: OK\n");
426
  }
427
}
428

    
429
void shellRequestResetCalibrationConstants(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
430
  chprintf(chp, "shellRequestResetCalibrationConstants\n");
431
  chprintf(chp, "Setting Ed=1.0f, Eb=1.0f\n");
432
  msg_t res;
433

    
434
  res = global.memory.setEd(1.0f);
435
  if (res != global.memory.OK)
436
    chprintf(chp, "Set Ed: FAIL\n");
437
  else
438
    chprintf(chp, "Set Ed: OK\n");
439

    
440
  res = global.memory.setEb(1.0f);
441
  if (res != global.memory.OK)
442
    chprintf(chp, "Set Eb: FAIL\n");
443
  else
444
    chprintf(chp, "Set Eb: OK\n");
445
}
446

    
447
void shellRequestGetCalibrationConstants(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
448
  chprintf(chp, "shellRequestGetCalibrationConstants\n");
449
  msg_t res;
450
  float Ed, Eb;
451

    
452
  res = global.memory.getEd(&Ed);
453
  if (res != global.memory.OK)
454
    chprintf(chp, "Get Ed: FAIL\n");
455
  else
456
    chprintf(chp, "Get Ed: OK \t Ed=%f\n", Ed);
457

    
458
  res = global.memory.getEb(&Eb);
459
  if (res != global.memory.OK)
460
    chprintf(chp, "Get Eb: FAIL\n");
461
  else
462
    chprintf(chp, "Get Eb: OK \t Eb=%f\n", Eb);
463
}
464

    
465
void shellRequestSetCalibrationConstants(BaseSequentialStream *chp, int argc, char *argv[]) {
466
  chprintf(chp, "shellRequestSetCalibrationConstants\n");
467
  msg_t res;
468

    
469
  if (argc != 3) {
470
    chprintf(chp, "Usage: %s\n","set_Ed_Eb <Ed> <Eb> <Write To Eeprom ? 1 : 0>");
471
    chprintf(chp, "(Call with floating point values for Ed and Eb values and write condition):\n");
472
    return;
473
  }
474
  // Get the write condition
475
  const float Ed = atof(argv[0]);
476
  const float Eb = atof(argv[1]);
477
  bool_t writeToMemory = atoi(argv[2]) == 1 ? true : false;
478

    
479
  res = global.motorcontrol.setWheelDiameterCorrectionFactor(Ed, writeToMemory);
480
  if (res != global.memory.OK)
481
    chprintf(chp, "Set Ed: FAIL\n");
482
  else
483
    chprintf(chp, "Set Ed: OK \t Ed=%f\n", Ed);
484

    
485
  res = global.motorcontrol.setActualWheelBaseDistance(Eb, writeToMemory);
486
  if (res != global.memory.OK)
487
    chprintf(chp, "Set Eb: FAIL\n");
488
  else
489
    chprintf(chp, "Set Eb: OK \t Ed=%f\n", Eb);
490
}
491

    
492
void shellRequestGetVcnl(BaseSequentialStream *chp, int argc, char *argv[]) {
493
  chprintf(chp, "shellRequestGetVcnl\n");
494
  // Print the sensor information
495
  if (argc != 1) {
496
    chprintf(chp, "Usage: %s\n","get_vcnl <rep>");
497
    return;
498
  }
499
  for (int32_t rep = 0x00; rep < atoi(argv[0]); ++rep) {
500
    for (uint8_t idx = 0x00; idx < global.vcnl4020.size(); idx++) {
501
     chprintf(chp, "%d: Ambi %d\tProx raw %d\tProx scaled %d\n", idx, global.vcnl4020[idx].getAmbientLight(), global.vcnl4020[idx].getProximity(), global.vcnl4020[idx].getProximityScaledWoOffset());
502
    }
503
    chprintf(chp, "\n\n");
504
    BaseThread::sleep(MS2ST(250));
505
  }
506
}
507

    
508
void shellRequestSetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
509
  chprintf(chp, "shellRequestSetVcnlOffset\n");
510
  if (argc != 2) {
511
    chprintf(chp, "Usage: %s\n","set_vcnl <idx> <offset>");
512
    return;
513
  }
514

    
515
  uint8_t vcnlIdx = static_cast<uint8_t>(atoi(argv[0]));
516
  uint16_t vcnlOffset = static_cast<uint16_t>(atoi(argv[1]));
517

    
518
  if (vcnlIdx >= global.vcnl4020.size()) {
519
    chprintf(chp, "Wrong VCNL index: Choose [0 .. %d]\n", global.vcnl4020.size()-1);
520
    return;
521
  }
522

    
523
  msg_t res = global.memory.setVcnl4020Offset(vcnlOffset, vcnlIdx);
524
  if (res != global.memory.OK) {
525
    chprintf(chp, "Set Offset: FAIL\n");
526
  } else {
527
    chprintf(chp, "Set Offset: OK\n");
528
    global.vcnl4020[vcnlIdx].setProximityOffset(vcnlOffset);
529
  }
530
}
531

    
532
void shellRequestResetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
533
  msg_t res = global.memory.OK;
534
  for (uint8_t idx = 0; idx < 4; ++idx) {
535
    msg_t r = global.memory.setVcnl4020Offset(0, idx);
536
    if (r == global.memory.OK) {
537
      global.vcnl4020[idx].setProximityOffset(0);
538
    } else {
539
      chprintf(chp, "Reset Offset %u: FAIL\n", idx);
540
      res = r;
541
    }
542
  }
543

    
544
  if (res == global.memory.OK) {
545
    chprintf(chp, "Reset Offset: DONE\n");
546
  }
547

    
548
  return;
549
}
550

    
551
void shellRequestGetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
552
  chprintf(chp, "shellRequestGetVcnlOffset\n");
553
  if (argc != 1) {
554
    chprintf(chp, "Call with decimal numbers: get_vcnl <idx>\n");
555
    return;
556
  }
557

    
558
  uint8_t vcnlIdx = static_cast<uint8_t>(atoi(argv[0]));
559

    
560
  if (vcnlIdx >= global.vcnl4020.size()) {
561
    chprintf(chp, "Wrong VCNL index: Choose [0 .. %d]\n", global.vcnl4020.size()-1);
562
    return;
563
  }
564

    
565
  uint16_t vcnlOffset;
566
  msg_t res = global.memory.getVcnl4020Offset(&vcnlOffset, vcnlIdx);
567
  if (res != global.memory.OK) {
568
    chprintf(chp, "Get Offset: FAIL\n");
569
  } else {
570
    chprintf(chp, "Get Offset: OK \t Offset=%d\n", vcnlOffset);
571
  }
572
}
573

    
574
void shellRequestCalib(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
575
  chprintf(chp, "shellRequestCalib\n");
576
  global.robot.calibrate();
577
}
578

    
579
void shellRequestGetRobotId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
580
  chprintf(chp, "shellRequestGetRobotId\n");
581
  chprintf(chp, "Robot ID: %u\n", global.robot.getRobotID());
582
  if (global.robot.getRobotID() == 0)
583
    chprintf(chp, "Warning: The board ID seems to be uninitialized.\n");
584
}
585

    
586
void shellRequestGetSystemLoad(BaseSequentialStream *chp, int argc, char *argv[]) {
587
  chprintf(chp, "shellRequestGetSystemLoad\n");
588
  uint8_t seconds = 1;
589
  if (argc >= 1) {
590
    seconds = atoi(argv[0]);
591
  }
592
  chprintf(chp, "measuring CPU load for %u %s...\n", seconds, (seconds>1)? "seconds" : "second");
593

    
594
  const systime_t before = chThdGetTicks(chSysGetIdleThread());
595
  BaseThread::sleep(S2ST(seconds));
596
  const systime_t after = chThdGetTicks(chSysGetIdleThread());
597
  const float usage = 1.0f - (float(after - before) / float(seconds * CH_FREQUENCY));
598

    
599
  chprintf(chp, "CPU load: %3.2f%%\n", usage * 100);
600
  const uint32_t memory_total = 0x10000;
601
  const uint32_t memory_load = memory_total - chCoreStatus();
602
  chprintf(chp, "RAM load: %3.2f%% (%u / %u Byte)\n", float(memory_load)/float(memory_total) * 100, memory_load, memory_total);
603
}
604

    
605
void shellSwitchBoardCmd(BaseSequentialStream *chp, int argc, char *argv[]) {
606
  if (argc != 1) {
607
    chprintf(chp, "Call with decimal numbers: shell_board <idx>\n");
608
    return;
609
  }
610
  uint8_t boardIdx = static_cast<uint8_t>(atoi(argv[0]));
611

    
612
  chprintf(chp, "shellSwitchBoardCmd\n");
613
  global.sercanmux1.sendSwitchCmd(boardIdx);
614
}
615

    
616
void shellRequestGetBootloaderInfo(BaseSequentialStream* chp, int argc, char *argv[]) {
617
  // check the magic number
618
  switch (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR))) {
619
    case (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0)):
620
      chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
621
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major,
622
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor,
623
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->patch);
624
      break;
625

    
626
    case BL_MAGIC_NUMBER:
627
      chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
628
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
629
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
630
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))));
631
      break;
632

    
633
    default:
634
      chprintf((BaseSequentialStream*) &SD1, "Bootloader incompatible\n");
635
      break;
636
  }
637

    
638
  return;
639
}
640

    
641
void shellRequestMotorDrive(BaseSequentialStream *chp, int argc, char *argv[]) {
642
  types::kinematic tmp;
643
  tmp.w_z = 0;
644
  tmp.x = 0;
645
  if (argc == 1){
646
    chprintf(chp, "Set speed to %i um/s \n", atoi(argv[0]));
647
    tmp.x = atoi(argv[0]);
648
  } else {
649
    if(argc == 2){
650
      chprintf(chp, "Set speed to %i \n um/s", atoi(argv[0]));
651
      chprintf(chp, "Set angular speed to %i \n urad/s", atoi(argv[1]));
652
      tmp.x = atoi(argv[0]);
653
      tmp.w_z= atoi(argv[1]);
654
    } else {
655
      chprintf(chp, "Wrong number of parameters given (%i), stopping robot \n", argc);
656
    }
657
  }
658

    
659
  global.motorcontrol.setTargetSpeed(tmp);
660
  return;
661
}
662

    
663
void shellRequestMotorStop(BaseSequentialStream *chp, int argc, char *argv[]) {
664
  types::kinematic tmp;
665
  tmp.x = 0;
666
  tmp.w_z = 0;
667

    
668
  global.motorcontrol.setTargetSpeed(tmp);
669

    
670
  chprintf(chp, "stop");
671
return;
672
}
673

    
674
void shellRequestMotorCalibrate(BaseSequentialStream *chp, int argc, char *argv[]) {
675
  global.motorcontrol.resetGains();
676
  chprintf((BaseSequentialStream*)&global.sercanmux1, "motor calibration starts in five seconds...\n");
677
  BaseThread::sleep(MS2ST(5000));
678
  global.motorcontrol.isCalibrating = true;
679

    
680
  return;
681
}
682

    
683
void shellRequestMotorGetGains(BaseSequentialStream *chp, int argc, char *argv[]){
684
  global.motorcontrol.printGains();
685

    
686
  return;
687
}
688

    
689
void shellRequestMotorResetGains(BaseSequentialStream *chp, int argc, char *argv[]) {
690
  global.motorcontrol.resetGains();;
691

    
692
  return;
693
}
694

    
695

    
696
/**
697
 * Calibrate the thresholds for left and right sensor to get the maximum threshold and to
698
 * be able to detect the correction direction.
699
 * In this case it is expected that the FL-Sensor sould be in the white part of the edge and the FR-Sensor in the black one.
700
 * 
701
 * Note: invert the threshs to drive on the other edge.
702
 * 
703
 * */
704
void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
705
    // int vcnl4020AmbientLight[4];
706
    int vcnl4020Proximity[4];
707
    int rounds = 1;
708
    int proxyL = 0;
709
    int proxyR = 0;
710
    int maxDelta = 0;
711
    int sensorL = 0;
712
    int sensorR = 0;
713
 
714
  if (argc == 1){
715
    chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
716
    rounds = atoi(argv[0]);
717
    
718
  }else{
719
    chprintf(chp, "Usage: calbrate_line_sensors [1,n]\nThis will calibrate the thresholds for the left and right sensor\naccording to the maximum delta value recorded.\n");
720
    return;
721
  }
722
  for (uint8_t led = 0; led < 8; ++led) {
723
    global.robot.setLightColor(led, Color(Color::BLACK));
724
  }
725

    
726
  for (int j = 0; j < rounds; j++) {
727
    for (int i = 0; i < 4; i++) {
728
        // vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
729
        vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
730
    }
731
    global.robot.setLightColor(j % 8, Color(Color::BLACK));
732
    global.robot.setLightColor(j+1 % 8, Color(Color::WHITE));
733
    int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
734
                  - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
735
    // Update proximity thresh
736
    if (delta > maxDelta) {
737
      for (uint8_t led = 0; led < 8; ++led) {
738
        global.robot.setLightColor(led, Color(Color::GREEN));
739
      }
740
      maxDelta = delta;
741
      proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
742
      proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
743
    }
744
    sensorL += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
745
    sensorR += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
746

    
747
    // if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
748
    //   delta *= -1;
749
    // }
750

    
751
    chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n", 
752
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
753
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
754
                  delta,
755
                  proxyL,
756
                  proxyR,
757
                  maxDelta);
758
    // sleep(CAN::UPDATE_PERIOD);
759
    BaseThread::sleep(CAN::UPDATE_PERIOD);
760
  }
761
  
762

    
763
  global.threshProxyL = sensorL / rounds;
764
  global.threshProxyR = sensorR / rounds;
765
  chprintf(chp,"Thresh FL: %d, FR: %d\n",  global.threshProxyL, global.threshProxyR);
766
  return;
767
}
768

    
769

    
770

    
771
void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
772
  // uint16_t vcnl4020AmbientLight[4];
773
  uint16_t vcnl4020Proximity[4];
774
  uint16_t rounds = 1;
775
  // uint16_t proxyL = global.threshProxyL;
776
  // uint16_t proxyR = global.threshProxyR;
777
  // uint16_t maxDelta = 0;
778
  
779
  // int sensorL = 0;
780
  // int sensorR = 0;
781
  if (argc == 1){
782
    chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
783
    rounds = atoi(argv[0]);
784
    
785
  }
786
  global.motorcontrol.getGains(&global.motorConfigGains);
787
  global.motorcontrol.setGains(&global.stopGains);
788

    
789
  for (int j = 0; j < rounds; j++) {
790
    for (int i = 0; i < 4; i++) {
791
        // vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
792
        vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
793
    }
794
    
795
    uint16_t delta = (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
796
                  - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
797
    // // Update proximity thresh
798
    // if (delta > maxDelta) {
799
    //   maxDelta = delta;
800
    //   proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
801
    //   proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
802
    // }
803

    
804
    // if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
805
    //   delta *= -1;
806
    // }
807

    
808
    chprintf(chp,"WL:%d,FL:%d,FR:%d,WR:%d,Delta:%d\n", 
809
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
810
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
811
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
812
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT],
813
                  delta);
814
    // sleep(CAN::UPDATE_PERIOD);
815
    BaseThread::sleep(CAN::UPDATE_PERIOD);
816
  }
817
  global.motorcontrol.setGains(&global.motorConfigGains);
818
  // chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR);
819
  return;
820
}
821

    
822
// Either 0 to disable record or > 0 to enable it
823
void setRecord(BaseSequentialStream *chp, int argc, char *argv[]){
824
  if (argc == 1){
825
    chprintf(chp, "Set recording to %d\n", atoi(argv[0]));
826
    global.enableRecord = atoi(argv[0]);
827
  }
828
}
829

    
830

    
831
void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
832
  // int vcnl4020AmbientLight[4];
833
  // int vcnl4020Proximity[4];
834
  int rpmSpeed[2] = {0};
835
  int steps = 0;
836
  // int proxyL = global.threshProxyL;
837
  // int proxyR = global.threshProxyR;
838
  int maxDelta = 0;
839
  float KCrit = 0.0f;
840
  global.sensSamples = 0;
841
  global.maxDist.error = 0;
842
  LineFollow lf(&global);
843
  int led = 0;
844
 
845
  if (argc == 2){
846
    chprintf(chp, "KCrti %f\n", atof(argv[0]));
847
    chprintf(chp, "Steps %i\n", atoi(argv[1]));
848
    KCrit = atof(argv[0]);
849
    steps = atoi(argv[1]);
850
  } else if (argc == 3){
851
    chprintf(chp, "KCrti %f\n", atof(argv[0]));
852
    chprintf(chp, "Steps %i\n", atoi(argv[1]));
853
    KCrit = atof(argv[0]);
854
    steps = atoi(argv[1]);
855
    global.forwardSpeed = atoi(argv[2]);
856
    
857
  }else{
858
    chprintf(chp, "Usage: dev_ziegler2 <K_crit> <steps> (<speed>)");
859
    return;
860
  }
861
  global.K_p = KCrit;
862
  for(led=0; led<8; led++){
863
        global.robot.setLightColor(led, Color(Color::BLACK));
864
  }
865
  chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n");
866
  BaseThread::sleep(MS2ST(5000));
867
  // global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
868
  int checkWhite = 0;
869
  int it_switch = steps / 2;
870
  // lf.setStrategie(LineFollowStrategie::MIDDLE);
871
  for(int s=0; s < steps; s++){
872
    
873
    checkWhite = lf.followLine(rpmSpeed);
874
    // chprintf(chp,"S:%d,",s);
875
    // if(global.threshWhite)
876
    // if(s < it_switch){
877
    //   lf.setStrategie(LineFollowStrategie::EDGE_LEFT);
878
    //   checkWhite = lf.followLine(rpmSpeed);
879
    // }else{
880
    //   lf.setStrategie(LineFollowStrategie::EDGE_RIGHT);
881
    //   checkWhite = lf.followLine(rpmSpeed);
882
    // }
883
    if(checkWhite){
884
      global.motorcontrol.setTargetRPM(0,0);
885
      for(led=0; led<8; led++){
886
        global.robot.setLightColor(led, Color(Color::RED));
887
      }
888
    }else{
889
      global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
890
    }
891
    
892
    
893
    BaseThread::sleep(CAN::UPDATE_PERIOD);
894
  }
895

    
896
  global.motorcontrol.setTargetRPM(0,0);
897
}
898

    
899

    
900
void followLine(BaseSequentialStream *chp, int argc, char *argv[]){
901
  int steps = 1000;
902
  int speed = 0;
903
  int strategy = 0;
904
  int led = 0;
905
  int checkWhite = 0;
906
  int rpmSpeed[2] = {0};
907
  LineFollow lf(&global);
908
  if (argc == 1){
909
      chprintf(chp, "%i steps \n", atoi(argv[0]));
910
      steps = atoi(argv[0]);
911
    }else if (argc == 2){
912
      steps = atoi(argv[0]);
913
      speed = atoi(argv[1]);
914
    }else if (argc == 3){
915
      steps = atoi(argv[0]);
916
      speed = atoi(argv[1]);
917
      strategy = atoi(argv[2]);
918
    }else{
919
      chprintf(chp, "Use: followLine <steps> <speed> <strategy>\n");
920
      return;
921
    }
922
    global.forwardSpeed = speed;
923
    switch (strategy)
924
    {
925
    case 0:
926
      lf.setStrategy(amiro::LineFollowStrategy::EDGE_RIGHT);
927
      break;
928
    case 1:
929
      lf.setStrategy(amiro::LineFollowStrategy::EDGE_LEFT);
930
      break;
931
    case 2:
932
      lf.setStrategy(amiro::LineFollowStrategy::FUZZY);
933
      break;
934
    default:
935
      break;
936
    }
937

    
938

    
939
    for(int s=0; s < steps; s++){
940
    
941
      checkWhite = lf.followLine(rpmSpeed);
942
      if(checkWhite){
943
        global.motorcontrol.setTargetRPM(0,0);
944
        for(led=0; led<8; led++){
945
          global.robot.setLightColor(led, Color(Color::RED));
946
        }
947
      }else{
948
        global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
949
      }
950
      
951
      BaseThread::sleep(CAN::UPDATE_PERIOD);
952
  }
953

    
954
  global.motorcontrol.setTargetRPM(0,0);
955
}
956

    
957

    
958
void printMove(BaseSequentialStream *chp, int argc, char *argv[]){
959

    
960
  for (int j=0; j<global.sensSamples;j++){
961
    chprintf(chp,"FL:%d,FR:%d,Delta:%d,Error:%d\n",global.senseRec[j].FL, global.senseRec[j].FR, global.senseRec[j].delta, global.senseRec[j].error);
962
  }
963
  chprintf(chp,"MaxDist: FL:%d,FR:%d,Delta:%d,Error:%d\n",global.maxDist.FL, global.maxDist.FR, global.maxDist.delta, global.maxDist.error);
964

    
965

    
966
}
967
void freeGains(BaseSequentialStream *chp, int argc, char *argv[]){
968
  if (global.resetProtect){
969
    global.motorcontrol.getGains(&global.motorConfigGains);
970
    global.resetProtect = 0;
971
  }
972
  
973
  global.motorcontrol.setGains(&global.stopGains);
974
}
975

    
976

    
977
void setGains(BaseSequentialStream *chp, int argc, char *argv[]){
978
  if(!global.resetProtect){
979
    global.motorcontrol.setGains(&global.motorConfigGains);
980
    global.resetProtect = 1;
981
  }
982
}
983
void motorToggle(BaseSequentialStream *chp, int argc, char *argv[]){
984
  global.odometry.resetPosition();
985
  global.motorcontrol.toggleMotorEnable();
986
}
987

    
988

    
989
void rotate(BaseSequentialStream *chp, int argc, char *argv[]){
990
  int steps = 1000;
991
  int speed = 0;
992
  int strategy = 0;
993
  int led = 0;
994
  int checkWhite = 0;
995
  int rpmSpeed[2] = {0};
996
  LineFollow lf(&global);
997
  if (argc == 1){
998
      chprintf(chp, "%i steps \n", atoi(argv[0]));
999
      speed = atoi(argv[0]);
1000
    }else if (argc == 2){
1001
      speed = atoi(argv[0]);
1002
      steps = atoi(argv[1]);
1003
    }else{
1004
      chprintf(chp, "Use: rotate <speed> <steps>\n");
1005
      return;
1006
    }
1007
    chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n");
1008
    BaseThread::sleep(MS2ST(5000));
1009
    // global.forwardSpeed = speed;
1010
  //   rpmSpeed[0] = -speed;
1011
  //   rpmSpeed[1] = speed;
1012
  //   global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
1013
  //   for(int s=0; s < steps; s++){
1014
  //     BaseThread::sleep(CAN::UPDATE_PERIOD);
1015
  // }
1016
  global.odometry.resetPosition();
1017
  global.startPos = global.odometry.getPosition();
1018
  global.distcontrol.setTargetPosition(0, 3141592, 5000);
1019
  BaseThread::sleep(MS2ST(11000));
1020
  global.endPos = global.odometry.getPosition();
1021
  // global.motorcontrol.setTargetRPM(0,0);
1022
}
1023

    
1024
void setSpeed(int (&rpmSpeed)[2]){
1025
  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
1026
}
1027

    
1028
/**
1029
 * Blocks as long as the position changes.
1030
 */
1031
void checkForMotion(){
1032
  int motion = 1;
1033
  int led = 0;
1034
  types::position oldPos = global.odometry.getPosition();
1035
  while(motion){
1036
    BaseThread::sleep(500);
1037
    types::position tmp = global.odometry.getPosition();
1038
    motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);
1039
    oldPos = tmp;
1040
      global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW));
1041
      global.robot.setLightColor(led % 8, Color(Color::BLACK));
1042
      led++;
1043
  }
1044
}
1045

    
1046
/**
1047
 * Turns MotorControl off and checks if position remains stable.
1048
 */
1049
int checkDockingSuccess(){
1050
  // global.motorcontrol.setTargetRPM(0,0);
1051
  checkForMotion();
1052
  int led = 0;
1053
  int success = 0;
1054
  global.odometry.resetPosition();
1055
  types::position start = global.startPos = global.odometry.getPosition();
1056
  global.motorcontrol.toggleMotorEnable();
1057
  BaseThread::sleep(1000);
1058
  types::position stop = global.endPos = global.odometry.getPosition();
1059
  global.motorcontrol.toggleMotorEnable();
1060
  // Amiro moved, docking was not successful
1061
  if ((start.x + stop.x) || (start.y + stop.y)){
1062
    for(led=0; led<8; led++){
1063
      global.robot.setLightColor(led, Color(Color::RED));
1064
    }
1065
    success = 0;
1066
  }else{
1067
    for(led=0; led<8; led++){
1068
      global.robot.setLightColor(led, Color(Color::GREEN));
1069
    }
1070
    success = 1;
1071
  }
1072
  
1073
  BaseThread::sleep(500);
1074
  for(led=0; led<8; led++){
1075
          global.robot.setLightColor(led, Color(Color::BLACK));
1076
  }
1077
  return success;
1078
}
1079

    
1080
/**
1081
 * Switch to EDGE_LEFT line following and orientate for n steps along the line.
1082
 * 
1083
 * @param lf  current line following
1084
 * @param steps steps to take for the correction (measured in CAN-update durations)
1085
 */
1086
void correctPosition(LineFollow &lf, int steps){
1087
  int checkWhite = 0;
1088
  int rpmSpeed[2] = {0};
1089
  lf.setStrategy(LineFollowStrategy::EDGE_LEFT);
1090
  for (int correction=0; correction<steps; correction++){
1091
    checkWhite = lf.followLine(rpmSpeed);
1092
    setSpeed(rpmSpeed);
1093
    BaseThread::sleep(CAN::UPDATE_PERIOD);
1094
  }
1095
}
1096

    
1097

    
1098

    
1099
void followAndRotate(BaseSequentialStream *chp, int argc, char *argv[]){
1100
  int steps = 10000;
1101
  int speed = 0;
1102
  int strategy = 0;
1103
  int led = 0;
1104
  int checkWhite = 0;
1105
  int rpmSpeed[2] = {0};
1106
  int proxyWheelThresh = 20000;
1107
  LineFollow lf(&global);
1108
  if (argc == 1){
1109
      chprintf(chp, "%i steps \n", atoi(argv[0]));
1110
      speed = atoi(argv[0]);
1111
    }else if (argc == 2){
1112
      speed = atoi(argv[0]);
1113
      steps = atoi(argv[1]);
1114
    }else{
1115
      chprintf(chp, "Use: rotate <speed> <steps>\n");
1116
      return;
1117
    }
1118
    chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n");
1119
    BaseThread::sleep(MS2ST(5000));
1120
    global.forwardSpeed = speed;
1121
    
1122
    setSpeed(rpmSpeed);
1123

    
1124

    
1125
    for(int s=0; s < steps; s++){
1126

    
1127
      int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
1128
      int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
1129
      if ((WL+WR) < proxyWheelThresh){
1130
        global.motorcontrol.setTargetRPM(0,0);
1131
        chprintf((BaseSequentialStream*)&global.sercanmux1, "Hit Break!\n");
1132
        if(lf.getStrategy() == LineFollowStrategy::DOCKING){
1133
          // Check if Docking was successful
1134
          if(checkDockingSuccess()){
1135
            break;
1136
          }else{
1137
            correctPosition(lf, 250);
1138
            lf.setStrategy(LineFollowStrategy::DOCKING);
1139
            // break;
1140
          }
1141
          
1142
        }else{
1143
          // global.motorcontrol.setTargetRPM(0,0);
1144
          BaseThread::sleep(1000);
1145
          // 180° Rotation 
1146
          global.distcontrol.setTargetPosition(0, 3141592, 10000);
1147
          // BaseThread::sleep(8000);
1148
          checkForMotion();
1149
          correctPosition(lf, 250);
1150
          // break;
1151
          lf.setStrategy(LineFollowStrategy::DOCKING);
1152
          
1153
        }
1154
      }
1155
      checkWhite = lf.followLine(rpmSpeed);
1156
      setSpeed(rpmSpeed);
1157
      BaseThread::sleep(CAN::UPDATE_PERIOD);
1158
  }
1159

    
1160
  
1161
}
1162

    
1163
void enableCharging(){
1164
  global.ltc4412.enable(true);
1165
}
1166

    
1167
void disableCharging(){
1168
  global.ltc4412.enable(false);
1169
}
1170

    
1171

    
1172
void setGlobalStrategy(BaseSequentialStream *chp, int argc, char *argv[]){
1173
  if(argc == 1){
1174
  
1175
    global.lfStrategy = atoi(argv[0]); 
1176
  }
1177
}
1178

    
1179
void getPosition(BaseSequentialStream *chp, int argc, char *argv[]){
1180
  types::position pos = global.odometry.getPosition();
1181
  chprintf(chp, "Start: Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d\n", global.startPos.x, global.startPos.y, global.startPos.f_x, global.startPos.f_y, global.startPos.f_z);
1182
  chprintf(chp, "End: Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d\n", global.endPos.x, global.endPos.y, global.endPos.f_x, global.endPos.f_y, global.endPos.f_z);
1183

    
1184
  if(argc == 1){
1185
    for (int i=0; i<atoi(argv[0]);i++){
1186
      types::position pos = global.odometry.getPosition();
1187
      chprintf(chp, "End: Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d\n", pos.x, pos.y, pos.f_x, pos.f_y, pos.f_z);
1188
      BaseThread::sleep(CAN::UPDATE_PERIOD);
1189
    }
1190
  }
1191
}
1192

    
1193
// TODO: Not wokring, either loading station has no power or logic not working
1194
void checkPower(BaseSequentialStream *chp, int argc, char *argv[]){
1195
  int steps = 2000;
1196
  int led = 0;
1197
  enableCharging();
1198
  for (int i=0; i<steps;i++){
1199
    chprintf(chp, "%s Enable: %s\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y2" : "n2"); 
1200
    if(global.ltc4412.isPluggedIn()){
1201
      // enableCharging();
1202
      for(led=0; led<8; led++){
1203
        global.robot.setLightColor(led, Color(Color::GREEN));
1204
      }
1205
    }else{
1206
      // disableCharging();
1207
      for(led=0; led<8; led++){
1208
        global.robot.setLightColor(led, Color(Color::RED));
1209
      }
1210
    }
1211
    BaseThread::sleep(CAN::UPDATE_PERIOD);
1212
  }
1213
  disableCharging();
1214
  for(led=0; led<8; led++){
1215
    global.robot.setLightColor(led, Color(Color::BLACK));
1216
  }
1217
}
1218

    
1219

    
1220
static const ShellCommand commands[] = {
1221
  {"shutdown", shellRequestShutdown},
1222
  {"wakeup", shellRequestWakeup},
1223
  {"check", shellRequestCheck},
1224
  {"reset_memory", shellRequestResetMemory},
1225
  {"get_board_id", shellRequestGetBoardId},
1226
  {"set_board_id", shellRequestSetBoardId},
1227
  {"get_memory_data", shellRequestGetMemoryData},
1228
  {"get_vcnl", shellRequestGetVcnl},
1229
  {"calib_vcnl_offset", shellRequestCalib},
1230
  {"set_vcnl_offset", shellRequestSetVcnlOffset},
1231
  {"reset_vcnl_offset", shellRequestResetVcnlOffset},
1232
  {"get_vcnl_offset", shellRequestGetVcnlOffset},
1233
  {"reset_Ed_Eb", shellRequestResetCalibrationConstants},
1234
  {"get_Ed_Eb", shellRequestGetCalibrationConstants},
1235
  {"set_Ed_Eb", shellRequestSetCalibrationConstants},
1236
  {"get_robot_id", shellRequestGetRobotId},
1237
  {"get_system_load", shellRequestGetSystemLoad},
1238
  {"set_lights", shellRequestSetLights},
1239
  {"shell_board", shellSwitchBoardCmd},
1240
  {"get_bootloader_info", shellRequestGetBootloaderInfo},
1241
  {"motor_drive", shellRequestMotorDrive},
1242
  {"motor_stop", shellRequestMotorStop},
1243
  {"motor_calibrate", shellRequestMotorCalibrate},
1244
  {"motor_getGains", shellRequestMotorGetGains},
1245
  {"motor_deactivate", freeGains},
1246
  {"motor_activate", setGains},
1247
  {"motor_resetGains", shellRequestMotorResetGains},
1248
  {"motorToggle", motorToggle},
1249
  {"dev_proxi_sensor_data", proxySensorData},
1250
  {"dev_ziegler2", zieglerMeth2},
1251
  // TODO: Stop user process from execution to finish/force calibration before anything starts
1252
  {"calibrate_line", calibrateLineSensores}, 
1253
  // {"record_move", recordMove},
1254
  {"print_record", printMove},
1255
  {"setRecord", setRecord},
1256
  {"followLine", followLine},
1257
  {"rotate", rotate},
1258
  {"followRotate", followAndRotate},
1259
  {"getPos", getPosition},
1260
  {"checkPower", checkPower},
1261
  {"setStrategy", setGlobalStrategy},
1262
  {NULL, NULL}
1263
};
1264

    
1265
static const ShellConfig shell_cfg1 = {
1266
  (BaseSequentialStream *) &global.sercanmux1,
1267
  commands
1268
};
1269

    
1270
void initPowermonitor(INA219::Driver &ina219, const float shuntResistance_O, const float maxExpectedCurrent_A, const uint16_t currentLsb_uA)
1271
{
1272
  INA219::CalibData calibData;
1273
  INA219::InitData initData;
1274

    
1275
  calibData.input.configuration.content.brng = INA219::Configuration::BRNG_16V;
1276
  calibData.input.configuration.content.pg = INA219::Configuration::PGA_40mV;
1277
  calibData.input.configuration.content.badc = INA219::Configuration::ADC_68100us;
1278
  calibData.input.configuration.content.sadc = INA219::Configuration::ADC_68100us;
1279
  calibData.input.configuration.content.mode = INA219::Configuration::MODE_ShuntBus_Continuous;
1280
  calibData.input.shunt_resistance_O = shuntResistance_O;
1281
  calibData.input.max_expected_current_A = maxExpectedCurrent_A;
1282
  calibData.input.current_lsb_uA = currentLsb_uA;
1283
  if (ina219.calibration(&calibData) != BaseSensor<>::SUCCESS)
1284
  {
1285
    chprintf((BaseSequentialStream*)&SD1, "WARNING: calibration of INA219 failed.\n");
1286
  }
1287

    
1288
  initData.configuration.value = calibData.input.configuration.value;
1289
  initData.calibration = calibData.output.calibration_value;
1290
  initData.current_lsb_uA = calibData.output.current_lsb_uA;
1291
  if (ina219.init(&initData) != BaseSensor<>::SUCCESS)
1292
  {
1293
    chprintf((BaseSequentialStream*)&SD1, "WARNING: initialization of INA219 failed.\n");
1294
  }
1295

    
1296
  if (calibData.input.current_lsb_uA != initData.current_lsb_uA)
1297
  {
1298
    chprintf((BaseSequentialStream*)&SD1, "NOTE: LSB for current measurement was limited when initializing INA219 (%u -> %u)", calibData.input.current_lsb_uA, initData.current_lsb_uA);
1299
  }
1300

    
1301
  return;
1302
}
1303

    
1304
/*
1305
 * Application entry point.
1306
 */
1307
int main(void) {
1308

    
1309
//  int16_t accel;
1310
  Thread *shelltp = NULL;
1311

    
1312
  /*
1313
   * System initializations.
1314
   * - HAL initialization, this also initializes the configured device drivers
1315
   *   and performs the board-specific initializations.
1316
   * - Kernel initialization, the main() function becomes a thread and the
1317
   *   RTOS is active.
1318
   */
1319
  halInit();
1320
  qeiInit();
1321
  System::init();
1322

    
1323
//  boardWakeup();
1324
//  boardWriteIoPower(1);
1325

    
1326
  /*
1327
   * Activates the serial driver 2 using the driver default configuration.
1328
   */
1329
  sdStart(&SD1, &global.sd1_config);
1330

    
1331
  chprintf((BaseSequentialStream*) &SD1, "\n");
1332
  chprintf((BaseSequentialStream*) &SD1, BOARD_NAME " " BOARD_VERSION "\n");
1333
  switch (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR))) {
1334
    case (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0)):
1335
      chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
1336
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major,
1337
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor,
1338
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->patch);
1339
      break;
1340

    
1341
    case BL_MAGIC_NUMBER:
1342
      chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
1343
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
1344
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
1345
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))));
1346
      break;
1347

    
1348
    default:
1349
      chprintf((BaseSequentialStream*) &SD1, "Bootloader incompatible\n");
1350
      break;
1351
  }
1352
  chprintf((BaseSequentialStream*) &SD1, "ChibiOS " CH_KERNEL_VERSION "\n");
1353
  // make sure that the info text is completetly printed
1354
  BaseThread::sleep(10);
1355

    
1356
  extStart(&EXTD1, &extcfg);
1357

    
1358
  /*
1359
   * Wait for a certain amount of time, so that the PowerBoard can activate
1360
   * the IO voltages for the I2C Bus
1361
   */
1362
  BaseThread::sleep(MS2ST(2000));
1363

    
1364
  boardClearI2CBus(GPIOB_COMPASS_SCL, GPIOB_COMPASS_SDA);
1365
  boardClearI2CBus(GPIOB_IR_SCL, GPIOB_IR_SDA);
1366

    
1367
  global.HW_I2C1.start(&global.i2c1_config);
1368
  global.HW_I2C2.start(&global.i2c2_config);
1369

    
1370
  global.memory.init();
1371

    
1372
  uint8_t i = 0;
1373
  if (global.memory.getBoardId(&i) == fileSystemIo::FileSystemIoBase::OK) {
1374
    chprintf((BaseSequentialStream*) &SD1, "Board ID: %u\n", i);
1375
  } else {
1376
    chprintf((BaseSequentialStream*) &SD1, "Error reading board ID\n");
1377
  }
1378
  chprintf((BaseSequentialStream*) &SD1, "\n");
1379

    
1380
  initPowermonitor(global.ina219, 0.1f, 0.075f, 10);
1381

    
1382
  for (i = 0x00u; i < global.vcnl4020.size(); i++) {
1383
    uint16_t buffer;
1384
    global.memory.getVcnl4020Offset(&buffer,i);
1385
    global.vcnl4020[i].setProximityOffset(buffer);
1386
    global.vcnl4020[i].start(NORMALPRIO);
1387
  }
1388

    
1389
  global.ina219.start(NORMALPRIO);
1390

    
1391
  global.hmc5883l.start(NORMALPRIO + 8);
1392

    
1393
  global.increments.start();  // Start the qei driver
1394

    
1395
  // Start the three axes gyroscope
1396
  global.l3g4200d.configure(&global.gyro_run_config);
1397
  global.l3g4200d.start(NORMALPRIO+5);
1398

    
1399
  global.odometry.start(NORMALPRIO + 20);
1400

    
1401
  global.robot.start(HIGHPRIO - 1);
1402

    
1403
  global.motorcontrol.start(NORMALPRIO + 7);
1404

    
1405
  global.distcontrol.start(NORMALPRIO + 9);
1406

    
1407
  // Set target velocity
1408
  types::kinematic velocity;
1409
  velocity.x = 0; // E.g.  "100*1e3" equals "10 cm/s"
1410
  velocity.w_z = 0; // E.g. "2*1e6" equals "2 rad/s"
1411
  global.motorcontrol.setTargetSpeed(velocity);
1412

    
1413
  // Start the three axes linear accelerometer
1414
  global.lis331dlh.configure(&global.accel_run_config);
1415
  global.lis331dlh.start(NORMALPRIO+4);
1416

    
1417
  // Start the user thread
1418
  global.userThread.start(NORMALPRIO);
1419

    
1420
  /* let the SYS_SYNC_N pin go, to signal that the initialization of the module is done */
1421
  palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_HIGH);
1422

    
1423
  /* wait until all modules are done */
1424
  while (palReadPad(GPIOC, GPIOC_SYS_INT_N) == PAL_LOW) {
1425
    continue;
1426
  }
1427

    
1428
  while (true) {
1429

    
1430
    if (!shelltp)
1431
      shelltp = shellCreate(&shell_cfg1, THD_WA_SIZE(1024), NORMALPRIO);
1432
    else if (chThdTerminated(shelltp)) {
1433
      chThdRelease(shelltp);    /* Recovers memory of the previous shell. */
1434
      shelltp = NULL;           /* Triggers spawning of a new shell.      */
1435
    }
1436

    
1437
    // Let the LED just blink as an alive signal
1438
    boardWriteLed(1);
1439
    BaseThread::sleep(MS2ST(250));
1440
    boardWriteLed(0);
1441
    BaseThread::sleep(MS2ST(250));
1442

    
1443
    if (shutdown_now != SHUTDOWN_NONE) {
1444
      if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0))) && (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != BL_MAGIC_NUMBER)) {
1445
        chprintf((BaseSequentialStream*) &SD1, "ERROR: unable to shut down (bootloader deprecated).\n");
1446
        shutdown_now = SHUTDOWN_NONE;
1447
      } else {
1448
        uint32_t blCallbackPtrAddr = BL_CALLBACK_TABLE_ADDR;
1449
        // handle bootloader version 0.2.x
1450
        if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) &&
1451
            (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2)) {
1452
          switch (shutdown_now) {
1453
            case SHUTDOWN_TRANSPORTATION:
1454
              blCallbackPtrAddr += 6 * 4;
1455
              break;
1456
            case SHUTDOWN_DEEPSLEEP:
1457
              blCallbackPtrAddr += 5 * 4;
1458
              break;
1459
            case SHUTDOWN_HIBERNATE:
1460
              blCallbackPtrAddr += 4 * 4;
1461
              break;
1462
            case SHUTDOWN_HANDLE_REQUEST:
1463
            case SHUTDOWN_RESTART:
1464
              blCallbackPtrAddr += 10 * 4;
1465
              break;
1466
            default:
1467
              blCallbackPtrAddr = 0;
1468
              break;
1469
          }
1470
        }
1471
        // handle bootloader version 0.3.x
1472
        else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) &&
1473
                 (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3)) {
1474
          switch (shutdown_now) {
1475
            case SHUTDOWN_TRANSPORTATION:
1476
              blCallbackPtrAddr += 6 * 4;
1477
              break;
1478
            case SHUTDOWN_DEEPSLEEP:
1479
              blCallbackPtrAddr += 5 * 4;
1480
              break;
1481
            case SHUTDOWN_HIBERNATE:
1482
              blCallbackPtrAddr += 4 * 4;
1483
              break;
1484
            case SHUTDOWN_RESTART:
1485
              blCallbackPtrAddr += 7 * 4;
1486
              break;
1487
            case SHUTDOWN_HANDLE_REQUEST:
1488
              blCallbackPtrAddr += 8 * 4;
1489
              break;
1490
            default:
1491
              blCallbackPtrAddr = 0;
1492
              break;
1493
          }
1494
        }
1495
        // handle bootloader version 1.0.x and 1.1.x
1496
        else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0))) &&
1497
                 ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major == 1 && (((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor == 0 || ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor == 1)) {
1498
          switch (shutdown_now) {
1499
            case SHUTDOWN_TRANSPORTATION:
1500
              blCallbackPtrAddr += 6 * 4;
1501
              break;
1502
            case SHUTDOWN_DEEPSLEEP:
1503
              blCallbackPtrAddr += 5 * 4;
1504
              break;
1505
            case SHUTDOWN_HIBERNATE:
1506
              blCallbackPtrAddr += 4 * 4;
1507
              break;
1508
            case SHUTDOWN_RESTART:
1509
              blCallbackPtrAddr += 7 * 4;
1510
              break;
1511
            case SHUTDOWN_HANDLE_REQUEST:
1512
              blCallbackPtrAddr += 8 * 4;
1513
              break;
1514
            default:
1515
              blCallbackPtrAddr = 0;
1516
              break;
1517
          }
1518
        }
1519

    
1520
        void (*blCallback)(void) = NULL;
1521
        if (blCallbackPtrAddr > BL_CALLBACK_TABLE_ADDR) {
1522
          blCallback = (void (*)(void))(*((uint32_t*)blCallbackPtrAddr));
1523

    
1524
          if (!blCallback) {
1525
            chprintf((BaseSequentialStream*) &SD1, "ERROR: Requested shutdown not supported.\n");
1526
            shutdown_now = SHUTDOWN_NONE;
1527
          } else {
1528
            chprintf((BaseSequentialStream*)&SD1, "initiating shutdown sequence...\n");
1529
            palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_LOW);
1530
            palWritePad(GPIOC, GPIOC_SYS_PD_N, PAL_LOW);
1531

    
1532
            chprintf((BaseSequentialStream*)&SD1, "stopping all threads and periphery...");
1533
            systemShutdown();
1534
            chprintf((BaseSequentialStream*)&SD1, "\tdone\n");
1535
            BaseThread::sleep(MS2ST(10)); // sleep to print everything
1536

    
1537
            blCallback();
1538
          }
1539

    
1540
        } else {
1541
          chprintf((BaseSequentialStream*) &SD1, "ERROR: invalid shutdown requested (%u).\n", shutdown_now);
1542
          shutdown_now = SHUTDOWN_NONE;
1543
        }
1544
      }
1545

    
1546
//    for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
1547
//        accel = lis331dlh.getAcceleration(i);
1548
//        chprintf((BaseSequentialStream*) &SD1, "%c%04X ", accel < 0 ? '-' : '+', accel < 0 ? -accel : accel);
1549
//    }
1550
//
1551
//    chprintf((BaseSequentialStream*) &SD1, "\n");
1552
//
1553
//    // Print out an alive signal
1554
//    chprintf((BaseSequentialStream*) &SD1, ".");
1555
    }
1556
  }
1557

    
1558
  return 0;
1559
}