Revision b4885314

View differences:

boards/DiWheelDrive/board.c
96 96 
  palSetPad(GPIOC, GPIOC_SYS_PD_N);
97 97 
}
98 98 

  		




  99
  
  
    
inline void boardClearI2CBus(const uint8_t scl_pad) {


  		




  
  99
  
    
inline void boardClearI2CBus(const uint8_t scl_pad, const uint8_t sda_pad) {


  		




  100
  100
  
    

  		




  101
  101
  
    
  uint8_t i;


  		




  102
  102
  
    

  		




  103
  
  
    
  // configure I²C SCL open drain


  		




  
  103
  
    
  // configure I²C SCL and SDA open drain


  		




  104
  104
  
    
  palSetPadMode(GPIOB, scl_pad, PAL_MODE_OUTPUT_OPENDRAIN);


  		




  
  105
  
    
  palSetPadMode(GPIOB, sda_pad, PAL_MODE_OUTPUT_OPENDRAIN);


  		




  105
  106
  
    

  		




  106
  
  
    
  // perform bus clear as per I²C Specification v5 3.1.16


  		




  107
  
  
    
  for (i = 0x00u; i < 0x09u; i++) {


  		




  
  107
  
    
  // perform a 2-wire software reset for the eeprom (see AT24C01BN-SH-B datasheet, chapter 3)


  		




  
  108
  
    
  // note: clock is ~50kHz (20us per cycle)


  		




  
  109
  
    
  palSetPad(GPIOB, sda_pad);


  		




  
  110
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  111
  
    
  chThdSleepMicroseconds(10);


  		




  
  112
  
    
  palSetPad(GPIOB, scl_pad);


  		




  
  113
  
    
  chThdSleepMicroseconds(5);


  		




  
  114
  
    
  palClearPad(GPIOB, sda_pad);


  		




  
  115
  
    
  chThdSleepMicroseconds(5);


  		




  
  116
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  117
  
    
  chThdSleepMicroseconds(5);


  		




  
  118
  
    
  palSetPad(GPIOB, sda_pad);


  		




  
  119
  
    
  chThdSleepMicroseconds(5);


  		




  
  120
  
    
  for (i = 0; i < 9; ++i) {


  		




  
  121
  
    
    palSetPad(GPIOB, scl_pad);


  		




  
  122
  
    
    chThdSleepMicroseconds(10);


  		




  
  123
  
    
    palClearPad(GPIOB, scl_pad);


  		




  
  124
  
    
    chThdSleepMicroseconds(10);


  		




  
  125
  
    
  }


  		




  
  126
  
    
  palSetPad(GPIOB, scl_pad);


  		




  
  127
  
    
  chThdSleepMicroseconds(5);


  		




  
  128
  
    
  palClearPad(GPIOB, sda_pad);


  		




  
  129
  
    
  chThdSleepMicroseconds(5);


  		




  
  130
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  131
  
    
  chThdSleepMicroseconds(10);


  		




  
  132
  
    
  palSetPad(GPIOB, scl_pad);


  		




  
  133
  
    
  chThdSleepMicroseconds(5);


  		




  
  134
  
    
  palSetPad(GPIOB, sda_pad);


  		




  
  135
  
    
  chThdSleepMicroseconds(5);


  		




  
  136
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  137
  
    
  chThdSleepMicroseconds(10);


  		




  
  138
  
    

  		




  
  139
  
    
  // perform bus clear as per I²C Specification v6 3.1.16


  		




  
  140
  
    
  // note: clock is 100kHz (10us per cycle)


  		




  
  141
  
    
  for (i = 0; i < 10; i++) {


  		




  108
  142
  
    
    palClearPad(GPIOB, scl_pad);


  		




  109
  143
  
    
    chThdSleepMicroseconds(5);


  		




  110
  144
  
    
    palSetPad(GPIOB, scl_pad);


  		




  ......




  113
  147
  
    

  		




  114
  148
  
    
  // reconfigure I²C SCL


  		




  115
  149
  
    
  palSetPadMode(GPIOB, scl_pad, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);


  		




  
  150
  
    
  palSetPadMode(GPIOB, sda_pad, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);


  		




  116
  151
  
    

  		




  
  152
  
    
  return;


  		




  117
  153
  
    
}


  		












  

    
      boards/DiWheelDrive/board.h
    
  





  198
  198
  
    
  void boardRequestShutdown(void);


  		




  199
  199
  
    
  void boardStandby(void);


  		




  200
  200
  
    
  void boardWakeup(void);


  		




  201
  
  
    
  void boardClearI2CBus(const uint8_t scl_pad);


  		




  
  201
  
    
  void boardClearI2CBus(const uint8_t scl_pad, const uint8_t sda_pad);


  		




  202
  202
  
    
#ifdef __cplusplus


  		




  203
  203
  
    
}


  		




  204
  204
  
    
#endif


  		












  

    
      boards/LightRing/board.c
    
  





  64
  64
  
    

  		




  65
  65
  
    
}


  		




  66
  66
  
    

  		




  67
  
  
    
inline void boardClearI2CBus(const uint8_t scl_pad) {


  		




  
  67
  
    
inline void boardClearI2CBus(const uint8_t scl_pad, const uint8_t sda_pad) {


  		




  68
  68
  
    

  		




  69
  69
  
    
  uint8_t i;


  		




  70
  70
  
    

  		




  71
  
  
    
  // configure I²C SCL open drain


  		




  
  71
  
    
  // configure I²C SCL and SDA open drain


  		




  72
  72
  
    
  palSetPadMode(GPIOB, scl_pad, PAL_MODE_OUTPUT_OPENDRAIN);


  		




  
  73
  
    
  palSetPadMode(GPIOB, sda_pad, PAL_MODE_OUTPUT_OPENDRAIN);


  		




  73
  74
  
    

  		




  74
  
  
    
  // perform bus clear as per I²C Specification v5 3.1.16


  		




  75
  
  
    
  for (i = 0x00u; i < 0x09u; i++) {


  		




  
  75
  
    
  // perform a 2-wire software reset for the eeprom (see AT24C01BN-SH-B datasheet, chapter 3)


  		




  
  76
  
    
  // note: clock is ~50kHz (20us per cycle)


  		




  
  77
  
    
  palSetPad(GPIOB, sda_pad);


  		




  
  78
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  79
  
    
  chThdSleepMicroseconds(10);


  		




  
  80
  
    
  palSetPad(GPIOB, scl_pad);


  		




  
  81
  
    
  chThdSleepMicroseconds(5);


  		




  
  82
  
    
  palClearPad(GPIOB, sda_pad);


  		




  
  83
  
    
  chThdSleepMicroseconds(5);


  		




  
  84
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  85
  
    
  chThdSleepMicroseconds(5);


  		




  
  86
  
    
  palSetPad(GPIOB, sda_pad);


  		




  
  87
  
    
  chThdSleepMicroseconds(5);


  		




  
  88
  
    
  for (i = 0; i < 9; ++i) {


  		




  
  89
  
    
    palSetPad(GPIOB, scl_pad);


  		




  
  90
  
    
    chThdSleepMicroseconds(10);


  		




  
  91
  
    
    palClearPad(GPIOB, scl_pad);


  		




  
  92
  
    
    chThdSleepMicroseconds(10);


  		




  
  93
  
    
  }


  		




  
  94
  
    
  palSetPad(GPIOB, scl_pad);


  		




  
  95
  
    
  chThdSleepMicroseconds(5);


  		




  
  96
  
    
  palClearPad(GPIOB, sda_pad);


  		




  
  97
  
    
  chThdSleepMicroseconds(5);


  		




  
  98
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  99
  
    
  chThdSleepMicroseconds(10);


  		




  
  100
  
    
  palSetPad(GPIOB, scl_pad);


  		




  
  101
  
    
  chThdSleepMicroseconds(5);


  		




  
  102
  
    
  palSetPad(GPIOB, sda_pad);


  		




  
  103
  
    
  chThdSleepMicroseconds(5);


  		




  
  104
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  105
  
    
  chThdSleepMicroseconds(10);


  		




  
  106
  
    

  		




  
  107
  
    
  // perform bus clear as per I²C Specification v6 3.1.16


  		




  
  108
  
    
  // note: clock is 100kHz (10us per cycle)


  		




  
  109
  
    
  for (i = 0; i < 10; i++) {


  		




  76
  110
  
    
    palClearPad(GPIOB, scl_pad);


  		




  77
  111
  
    
    chThdSleepMicroseconds(5);


  		




  78
  112
  
    
    palSetPad(GPIOB, scl_pad);


  		




  ......




  81
  115
  
    

  		




  82
  116
  
    
  // reconfigure I²C SCL


  		




  83
  117
  
    
  palSetPadMode(GPIOB, scl_pad, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);


  		




  
  118
  
    
  palSetPadMode(GPIOB, sda_pad, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);


  		




  84
  119
  
    

  		




  
  120
  
    
  return;


  		




  85
  121
  
    
}


  		












  

    
      boards/LightRing/board.h
    
  





  178
  178
  
    
  void boardInit(void);


  		




  179
  179
  
    
  void boardRequestShutdown(void);


  		




  180
  180
  
    
  void boardStandby(void);


  		




  181
  
  
    
  void boardClearI2CBus(const uint8_t scl_pad);


  		




  
  181
  
    
  void boardClearI2CBus(const uint8_t scl_pad, const uint8_t sda_pad);


  		




  182
  182
  
    
#ifdef __cplusplus


  		




  183
  183
  
    
}


  		




  184
  184
  
    
#endif


  		












  

    
      boards/PowerManagement/board.c
    
  





  146
  146
  
    
  palSetPad(GPIOC, GPIOC_SYS_PD_N);


  		




  147
  147
  
    
}


  		




  148
  148
  
    

  		




  149
  
  
    
inline void boardClearI2CBus(const uint8_t scl_pad) {


  		




  
  149
  
    
inline void boardClearI2CBus(const uint8_t scl_pad, const uint8_t sda_pad) {


  		




  150
  150
  
    

  		




  151
  151
  
    
  uint8_t i;


  		




  152
  152
  
    

  		




  153
  
  
    
  // configure I²C SCL open drain


  		




  
  153
  
    
  // configure I²C SCL and SDA open drain


  		




  154
  154
  
    
  palSetPadMode(GPIOB, scl_pad, PAL_MODE_OUTPUT_OPENDRAIN);


  		




  
  155
  
    
  palSetPadMode(GPIOB, sda_pad, PAL_MODE_OUTPUT_OPENDRAIN);


  		




  155
  156
  
    

  		




  156
  
  
    
  // perform bus clear as per I²C Specification v5 3.1.16


  		




  157
  
  
    
  for (i = 0x00u; i < 0x09u; i++) {


  		




  
  157
  
    
  // perform a 2-wire software reset for the eeprom (see AT24C01BN-SH-B datasheet, chapter 3)


  		




  
  158
  
    
  // note: clock is ~50kHz (20us per cycle)


  		




  
  159
  
    
  palSetPad(GPIOB, sda_pad);


  		




  
  160
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  161
  
    
  chThdSleepMicroseconds(10);


  		




  
  162
  
    
  palSetPad(GPIOB, scl_pad);


  		




  
  163
  
    
  chThdSleepMicroseconds(5);


  		




  
  164
  
    
  palClearPad(GPIOB, sda_pad);


  		




  
  165
  
    
  chThdSleepMicroseconds(5);


  		




  
  166
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  167
  
    
  chThdSleepMicroseconds(5);


  		




  
  168
  
    
  palSetPad(GPIOB, sda_pad);


  		




  
  169
  
    
  chThdSleepMicroseconds(5);


  		




  
  170
  
    
  for (i = 0; i < 9; ++i) {


  		




  
  171
  
    
    palSetPad(GPIOB, scl_pad);


  		




  
  172
  
    
    chThdSleepMicroseconds(10);


  		




  
  173
  
    
    palClearPad(GPIOB, scl_pad);


  		




  
  174
  
    
    chThdSleepMicroseconds(10);


  		




  
  175
  
    
  }


  		




  
  176
  
    
  palSetPad(GPIOB, scl_pad);


  		




  
  177
  
    
  chThdSleepMicroseconds(5);


  		




  
  178
  
    
  palClearPad(GPIOB, sda_pad);


  		




  
  179
  
    
  chThdSleepMicroseconds(5);


  		




  
  180
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  181
  
    
  chThdSleepMicroseconds(10);


  		




  
  182
  
    
  palSetPad(GPIOB, scl_pad);


  		




  
  183
  
    
  chThdSleepMicroseconds(5);


  		




  
  184
  
    
  palSetPad(GPIOB, sda_pad);


  		




  
  185
  
    
  chThdSleepMicroseconds(5);


  		




  
  186
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  187
  
    
  chThdSleepMicroseconds(10);


  		




  
  188
  
    

  		




  
  189
  
    
  // perform bus clear as per I²C Specification v6 3.1.16


  		




  
  190
  
    
  // note: clock is 100kHz (10us per cycle)


  		




  
  191
  
    
  for (i = 0; i < 10; i++) {


  		




  158
  192
  
    
    palClearPad(GPIOB, scl_pad);


  		




  159
  193
  
    
    chThdSleepMicroseconds(5);


  		




  160
  194
  
    
    palSetPad(GPIOB, scl_pad);


  		




  ......




  163
  197
  
    

  		




  164
  198
  
    
  // reconfigure I²C SCL


  		




  165
  199
  
    
  palSetPadMode(GPIOB, scl_pad, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN);


  		




  
  200
  
    
  palSetPadMode(GPIOB, sda_pad, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN);


  		




  
  201
  
    

  		




  
  202
  
    
  return;


  		




  
  203
  
    
}


  		




  
  204
  
    

  		




  
  205
  
    
inline void boardResetBQ27500I2C(const uint8_t scl_pad, const uint8_t sda_pad) {


  		




  
  206
  
    

  		




  
  207
  
    
  // configure I²C SCL and SDA open drain


  		




  
  208
  
    
  palSetPadMode(GPIOB, scl_pad, PAL_MODE_OUTPUT_OPENDRAIN);


  		




  
  209
  
    
  palSetPadMode(GPIOB, sda_pad, PAL_MODE_OUTPUT_OPENDRAIN);


  		




  
  210
  
    

  		




  
  211
  
    
  // BQ27500: reset by holding bus low for t_BUSERR (17.3 - 21.2 seconds)


  		




  
  212
  
    
  palClearPad(GPIOB, scl_pad);


  		




  
  213
  
    
  palClearPad(GPIOB, sda_pad);


  		




  
  214
  
    
  chThdSleepSeconds(20);


  		




  
  215
  
    

  		




  
  216
  
    
  boardClearI2CBus(scl_pad, sda_pad);


  		




  166
  217
  
    

  		




  
  218
  
    
  return;


  		




  167
  219
  
    
}


  		












  

    
      boards/PowerManagement/board.h
    
  





  375
  375
  
    
  void boardStandby(void);


  		




  376
  376
  
    
  void boardStop(const uint8_t lpds, const uint8_t fpds);


  		




  377
  377
  
    
  void boardWakeup(void);


  		




  378
  
  
    
  void boardClearI2CBus(const uint8_t scl_pad);


  		




  
  378
  
    
  void boardClearI2CBus(const uint8_t scl_pad, const uint8_t sda_pad);


  		




  
  379
  
    
  void boardResetBQ27500I2C(const uint8_t scl_pad, const uint8_t sda_pad);


  		




  379
  380
  
    
#ifdef __cplusplus


  		




  380
  381
  
    
}


  		




  381
  382
  
    
#endif


  		












  

    
      components/ControllerAreaNetworkRx.cpp
    
  





  99
  99
  
    
      }


  		




  100
  100
  
    
      break;


  		




  101
  101
  
    

  		




  
  102
  
    
    case CAN::MAGNETOMETER_X_ID:


  		




  
  103
  
    
      if (frame->DLC == 4) {


  		




  
  104
  
    
        magnetometerValue[0] = frame->data32[0];


  		




  
  105
  
    
        return RDY_OK;


  		




  
  106
  
    
      }


  		




  
  107
  
    
      break;


  		




  
  108
  
    

  		




  
  109
  
    
    case CAN::MAGNETOMETER_Y_ID:


  		




  
  110
  
    
      if (frame->DLC == 4) {


  		




  
  111
  
    
        magnetometerValue[1] = frame->data32[0];


  		




  
  112
  
    
        return RDY_OK;


  		




  
  113
  
    
      }


  		




  
  114
  
    
      break;


  		




  
  115
  
    

  		




  
  116
  
    
    case CAN::MAGNETOMETER_Z_ID:


  		




  
  117
  
    
      if (frame->DLC == 4) {


  		




  
  118
  
    
        magnetometerValue[2] = frame->data32[0];


  		




  
  119
  
    
        return RDY_OK;


  		




  
  120
  
    
      }


  		




  
  121
  
    
      break;


  		




  
  122
  
    

  		




  
  123
  
    
    case CAN::GYROSCOPE_ID:


  		




  
  124
  
    
      if (frame->DLC == 6) {


  		




  
  125
  
    
        gyroscopeValue[0] = frame->data16[0];


  		




  
  126
  
    
        gyroscopeValue[1] = frame->data16[1];


  		




  
  127
  
    
        gyroscopeValue[2] = frame->data16[2];


  		




  
  128
  
    
        return RDY_OK;


  		




  
  129
  
    
      }


  		




  
  130
  
    
      break;


  		




  
  131
  
    

  		




  102
  132
  
    
    default:


  		




  103
  133
  
    
      break;


  		




  104
  134
  
    
  }


  		




  ......




  127
  157
  
    
  return this->robotId;


  		




  128
  158
  
    
}


  		




  129
  159
  
    

  		




  
  160
  
    
int32_t ControllerAreaNetworkRx::getMagnetometerValue(int axis)


  		




  
  161
  
    
{


  		




  
  162
  
    
  return this->magnetometerValue[axis];


  		




  
  163
  
    
}


  		




  
  164
  
    

  		




  
  165
  
    
int16_t ControllerAreaNetworkRx::getGyroscopeValue(int axis)


  		




  
  166
  
    
{


  		




  
  167
  
    
  return this->gyroscopeValue[axis];


  		




  
  168
  
    
}


  		




  
  169
  
    

  		




  130
  170
  
    

  		




  131
  171
  
    
uint16_t ControllerAreaNetworkRx::getProximityFloorValue(int index) {


  		




  132
  172
  
    
  return this->proximityFloorValue[index];


  		












  

    
      components/Odometry.cpp
    
  





  15
  15
  
    
using namespace constants::DiWheelDrive;


  		




  16
  16
  
    

  		




  17
  17
  
    

  		




  18
  
  
    
Odometry::Odometry(MotorIncrements* mi)


  		




  
  18
  
    
Odometry::Odometry(MotorIncrements* mi, L3G4200D* gyroscope)


  		




  19
  19
  
    
    : BaseStaticThread<512>(),


  		




  20
  20
  
    
      motorIncrements(mi),


  		




  
  21
  
    
      gyro(gyroscope),


  		




  21
  22
  
    
      eventSource(),


  		




  22
  23
  
    
      period(50),


  		




  23
  24
  
    
      incrementsPerRevolution(incrementsPerRevolution),


  		




  ......




  101
  102
  
    
  while (!this->shouldTerminate()) {


  		




  102
  103
  
    
    time += MS2ST(this->period);


  		




  103
  104
  
    

  		




  104
  
  
    
    // Update the base distance, because it may change after an calibration


  		




  
  105
  
    
    // Update the base distance, because it may change after a calibration


  		




  105
  106
  
    
    this->updateWheelBaseDistance();


  		




  106
  107
  
    

  		




  107
  108
  
    
    // Get the actual speed


  		




  ......




  109
  110
  
    

  		




  110
  111
  
    
    // Calculate the odometry


  		




  111
  112
  
    
    this->updateOdometry();


  		




  
  113
  
    
		


  		




  112
  114
  
    

  		




  113
  115
  
    
//     chprintf((BaseSequentialStream*) &SD1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);


  		




  114
  116
  
    
//     chprintf((BaseSequentialStream*) &SD1, "\r\n");


  		




  115
  117
  
    
//     chprintf((BaseSequentialStream*) &SD1, "distance_left:%f distance_right:%f", this->distance[0],this->distance[1]);


  		




  116
  118
  
    
//     chprintf((BaseSequentialStream*) &SD1, "\r\n");


  		




  117
  119
  
    

  		




  118
  
  
    
    chThdSleepUntil(time);


  		




  
  120
  
    
	if (time >= System::getTime()) {


  		




  
  121
  
    
      chThdSleepUntil(time);


  		




  
  122
  
    
    } else {


  		




  
  123
  
    
      chprintf((BaseSequentialStream*) &SD1, "WARNING Odometry: Unable to keep track\r\n");


  		




  
  124
  
    
	}


  		




  119
  125
  
    
  }


  		




  120
  126
  
    

  		




  121
  127
  
    
  return true;


  		




  ......




  125
  131
  
    

  		




  126
  132
  
    
  // Get the temporary position and error


  		




  127
  133
  
    
  float Cp3x3[9];


  		




  
  134
  
    
  uint32_t angular_ud;


  		




  128
  135
  
    
  chSysLock();


  		




  129
  136
  
    
    float pX = this->pX;


  		




  130
  137
  
    
    float pY = this->pY;


  		




  131
  138
  
    
    float pPhi = this->pPhi;


  		




  132
  139
  
    
    Matrix::copy<float>(this->Cp3x3,3,3,Cp3x3,3,3);


  		




  
  140
  
    
    // Get the differentail gyro information and reset it


  		




  
  141
  
    
    angular_ud = gyro->getAngular_ud(L3G4200D::AXIS_Z);


  		




  
  142
  
    
	gyro->angularReset();


  		




  133
  143
  
    
  chSysUnlock();


  		




  134
  144
  
    

  		




  135
  145
  
    
  ////////////////


  		




  ......




  137
  147
  
    
  ////////////////


  		




  138
  148
  
    

  		




  139
  149
  
    
  // TMP: Rotated angular


  		




  140
  
  
    
  float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;


  		




  
  150
  
    
  // float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;


  		




  
  151
  
    
  float dPhi = ((float(angular_ud * 1e-3) * M_PI ) * 1e-3) / 180.0f;


  		




  141
  152
  
    

  		




  142
  153
  
    
  // TMP: Moved distance


  		




  143
  154
  
    
  float dDistance = (this->distance[RIGHT_WHEEL] + this->distance[LEFT_WHEEL]) / 2.0f;


  		












  

    
      components/accel/lis331dlh.cpp
    
  





  5
  5
  
    
#include <amiro/accel/lis331dlh.hpp>


  		




  6
  6
  
    
#include <chprintf.h>


  		




  7
  7
  
    
#include <cmath>  // abs()


  		




  
  8
  
    
#include <amiro/Constants.h>


  		




  8
  9
  
    

  		




  9
  10
  
    
namespace amiro {


  		




  10
  11
  
    

  		




  ......




  72
  73
  
    

  		




  73
  74
  
    
    this->eventSource.broadcastFlags(0);


  		




  74
  75
  
    

  		




  75
  
  
    
    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200));


  		




  
  76
  
    
    this->waitAnyEventTimeout(ALL_EVENTS, CAN::UPDATE_PERIOD_MSEC);


  		




  76
  77
  
    

  		




  77
  78
  
    
  }


  		




  78
  79
  
    
  return RDY_OK;


  		












  

    
      components/gyro/l3g4200d.cpp
    
  





  
  1
  
    
#include <ch.hpp>


  		




  
  2
  
    
#include <hal.h>


  		




  
  3
  
    
#include <chprintf.h>


  		




  1
  4
  
    
#include <string.h>


  		




  2
  5
  
    

  		




  3
  6
  
    
#include <amiro/util/util.h>


  		




  4
  7
  
    
#include <amiro/bus/spi/HWSPIDriver.hpp>


  		




  5
  8
  
    
#include <amiro/gyro/l3g4200d.hpp>


  		




  
  9
  
    
#include <amiro/Constants.h>


  		




  
  10
  
    

  		




  
  11
  
    
using namespace chibios_rt;


  		




  6
  12
  
    

  		




  7
  13
  
    
namespace amiro {


  		




  8
  14
  
    

  		




  9
  15
  
    
L3G4200D::L3G4200D(HWSPIDriver *driver)


  		




  10
  
  
    
    : driver(driver) {


  		




  11
  
  
    

  		




  
  16
  
    
    : driver(driver),


  		




  
  17
  
    
      udpsPerTic(175000),


  		




  
  18
  
    
      period_us(100000) {


  		




  
  19
  
    
  this->period_ms = this->period_us * 1e-3;


  		




  
  20
  
    
  this->period_st = US2ST(this->period_us);


  		




  12
  21
  
    
}


  		




  13
  22
  
    

  		




  14
  23
  
    
L3G4200D::~L3G4200D() {


  		




  ......




  21
  30
  
    
}


  		




  22
  31
  
    

  		




  23
  32
  
    
msg_t L3G4200D::main() {


  		




  24
  
  
    

  		




  25
  
  
    
  this->setName("L3g4200d");


  		




  
  33
  
    
  systime_t time = System::getTime();


  		




  
  34
  
    
  this->setName("l3g4200d");


  		




  26
  35
  
    

  		




  27
  36
  
    
  while (!this->shouldTerminate()) {


  		




  
  37
  
    
	time += this->period_st;


  		




  28
  38
  
    

  		




  29
  39
  
    
    updateSensorData();


  		




  
  40
  
    
    calcAngular();


  		




  30
  41
  
    

  		




  31
  42
  
    
    this->eventSource.broadcastFlags(1);


  		




  32
  43
  
    

  		




  33
  
  
    
    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200));


  		




  34
  
  
    

  		




  
  44
  
    
    if (time >= System::getTime()) {


  		




  
  45
  
    
      chThdSleepUntil(time);


  		




  
  46
  
    
    } else {


  		




  
  47
  
    
      chprintf((BaseSequentialStream*) &SD1, "WARNING l3g4200d: Unable to keep track\r\n");


  		




  
  48
  
    
	}


  		




  35
  49
  
    
  }


  		




  36
  50
  
    
  return RDY_OK;


  		




  37
  51
  
    
}


  		




  ......




  39
  53
  
    
int16_t


  		




  40
  54
  
    
L3G4200D::


  		




  41
  55
  
    
getAngularRate(const uint8_t axis) {


  		




  42
  
  
    

  		




  43
  56
  
    
  return this->angularRate[axis];


  		




  
  57
  
    
}


  		




  44
  58
  
    

  		




  
  59
  
    
int32_t


  		




  
  60
  
    
L3G4200D::


  		




  
  61
  
    
getAngularRate_udps(const uint8_t axis) {


  		




  
  62
  
    
  return uint32_t(this->angularRate[axis]) * this->udpsPerTic;


  		




  
  63
  
    
}


  		




  
  64
  
    

  		




  
  65
  
    
int32_t


  		




  
  66
  
    
L3G4200D::


  		




  
  67
  
    
getAngular(const uint8_t axis) {


  		




  
  68
  
    
  return this->angular[axis];


  		




  
  69
  
    
}


  		




  
  70
  
    

  		




  
  71
  
    
int32_t


  		




  
  72
  
    
L3G4200D::


  		




  
  73
  
    
getAngular_ud(const uint8_t axis) {


  		




  
  74
  
    
  const int32_t angularRate_mdps = this->getAngularRate_udps(axis) * 1e-3;


  		




  
  75
  
    
  return angularRate_mdps * (this->integrationTic * this->period_ms);


  		




  45
  76
  
    
}


  		




  46
  77
  
    

  		




  
  78
  
    
void


  		




  
  79
  
    
L3G4200D::


  		




  
  80
  
    
calcAngular() {


  		




  
  81
  
    
  // Need to check for overflow!


  		




  
  82
  
    
  ++this->integrationTic;


  		




  
  83
  
    
  chSysLock();


  		




  
  84
  
    
  this->angular[L3G4200D::AXIS_X] += int32_t(this->angularRate[L3G4200D::AXIS_X]);


  		




  
  85
  
    
  this->angular[L3G4200D::AXIS_Y] += int32_t(this->angularRate[L3G4200D::AXIS_Y]);


  		




  
  86
  
    
  this->angular[L3G4200D::AXIS_Z] += int32_t(this->angularRate[L3G4200D::AXIS_Z]);


  		




  
  87
  
    
  chSysUnlock();


  		




  
  88
  
    
}


  		




  
  89
  
    

  		




  
  90
  
    
// TODO: Outsource, so that everyone who needs this has an own instance of the integrator


  		




  
  91
  
    
void


  		




  
  92
  
    
L3G4200D::


  		




  
  93
  
    
angularReset() {


  		




  
  94
  
    
  this->angular[L3G4200D::AXIS_X] = 0;


  		




  
  95
  
    
  this->angular[L3G4200D::AXIS_Y] = 0;


  		




  
  96
  
    
  this->angular[L3G4200D::AXIS_Z] = 0;


  		




  
  97
  
    
  this->integrationTic = 0;


  		




  
  98
  
    
}


  		




  
  99
  
    

  		




  
  100
  
    

  		




  47
  101
  
    
void L3G4200D::updateSensorData() {


  		




  48
  102
  
    

  		




  49
  103
  
    
  const size_t buffer_size = offsetof(L3G4200D::registers, OUT_Z)


  		




  ......




  62
  116
  
    
  // assemble data


  		




  63
  117
  
    
  sreg = buffer[1];


  		




  64
  118
  
    

  		




  
  119
  
    
  chSysLock();


  		




  65
  120
  
    
  if (sreg & L3G4200D::XDA)


  		




  66
  121
  
    
    this->angularRate[L3G4200D::AXIS_X] = (buffer[3] << 8) + buffer[2];


  		




  67
  122
  
    

  		




  ......




  70
  125
  
    

  		




  71
  126
  
    
  if (sreg & L3G4200D::ZDA)


  		




  72
  127
  
    
    this->angularRate[L3G4200D::AXIS_Z] = (buffer[7] << 8) + buffer[6];


  		




  73
  
  
    

  		




  
  128
  
    
  chSysUnlock();


  		




  74
  129
  
    
}


  		




  75
  130
  
    

  		




  76
  131
  
    
msg_t L3G4200D::configure(const L3G4200DConfig *config) {


  		




  ......




  95
  150
  
    
  buffer[5] = config->ctrl5;


  		




  96
  151
  
    
  this->driver->write(buffer, 6);


  		




  97
  152
  
    

  		




  
  153
  
    
  // Handle the new update time


  		




  
  154
  
    
  switch(config->ctrl1 & L3G4200D::DR_MASK) {


  		




  
  155
  
    
    case L3G4200D::DR_100_HZ: this->period_us = 10000; break;


  		




  
  156
  
    
    case L3G4200D::DR_200_HZ: this->period_us =  5000; break;


  		




  
  157
  
    
    case L3G4200D::DR_400_HZ: this->period_us =  2500; break;


  		




  
  158
  
    
    case L3G4200D::DR_800_HZ: this->period_us =  1250; break;


  		




  
  159
  
    
  }


  		




  
  160
  
    
  this->period_st = US2ST(this->period_st);


  		




  
  161
  
    

  		




  
  162
  
    
  // Handle the new full scale


  		




  
  163
  
    
  switch(config->ctrl1 & L3G4200D::FS_MASK) {


  		




  
  164
  
    
    case L3G4200D::FS_250_DPS:  this->udpsPerTic =  8750; break;


  		




  
  165
  
    
    case L3G4200D::FS_500_DPS:  this->udpsPerTic = 17500; break;


  		




  
  166
  
    
    case L3G4200D::FS_2000_DPS: this->udpsPerTic = 70000; break;


  		




  
  167
  
    
  }


  		




  
  168
  
    

  		




  
  169
  
    
  // Reset the integration


  		




  
  170
  
    
  this->angularReset();


  		




  
  171
  
    
    


  		




  98
  172
  
    
  return RDY_OK;


  		




  99
  173
  
    

  		




  100
  174
  
    
}


  		












  

    
      components/input/mpr121.cpp
    
  





  1
  1
  
    
#include <amiro/bus/i2c/I2CDriver.hpp>


  		




  2
  2
  
    
#include <amiro/input/mpr121.hpp>


  		




  
  3
  
    
#include <amiro/Constants.h>


  		




  3
  4
  
    

  		




  4
  5
  
    
namespace amiro {


  		




  5
  6
  
    

  		




  ......




  171
  172
  
    

  		




  172
  173
  
    
    this->eventSource.broadcastFlags(0);


  		




  173
  174
  
    

  		




  174
  
  
    
    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(100));


  		




  
  175
  
    
    this->waitAnyEventTimeout(ALL_EVENTS, CAN::UPDATE_PERIOD_MSEC);


  		




  175
  176
  
    
  }


  		




  176
  177
  
    

  		




  177
  178
  
    
  return RDY_OK;


  		












  

    
      components/magneto/hmc5883l.cpp
    
  





  5
  5
  
    
#include <amiro/bus/i2c/I2CParams.hpp>


  		




  6
  6
  
    
#include <amiro/bus/i2c/I2CDriver.hpp>


  		




  7
  7
  
    
#include <amiro/magneto/hmc5883l.hpp>


  		




  
  8
  
    
#include <amiro/Constants.h>


  		




  8
  9
  
    

  		




  9
  10
  
    
using namespace chibios_rt;


  		




  10
  11
  
    

  		




  ......




  50
  51
  
    

  		




  51
  52
  
    
    this->eventSource.broadcastFlags(0);


  		




  52
  53
  
    

  		




  53
  
  
    
    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200));


  		




  
  54
  
    
    this->waitAnyEventTimeout(ALL_EVENTS, CAN::UPDATE_PERIOD_MSEC);


  		




  54
  55
  
    
  }


  		




  55
  56
  
    

  		




  56
  57
  
    
  return RDY_OK;


  		












  

    
      components/proximity/vcnl4020.cpp
    
  





  6
  6
  
    
#include <amiro/bus/i2c/I2CParams.hpp>


  		




  7
  7
  
    
#include <amiro/bus/i2c/I2CDriver.hpp>


  		




  8
  8
  
    
#include <amiro/proximity/vcnl4020.hpp>


  		




  
  9
  
    
#include <amiro/Constants.h>


  		




  9
  10
  
    

  		




  10
  11
  
    
using namespace chibios_rt;


  		




  11
  12
  
    

  		




  ......




  107
  108
  
    

  		




  108
  109
  
    
    this->eventSource.broadcastFlags(0);


  		




  109
  110
  
    

  		




  110
  
  
    
    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200));


  		




  
  111
  
    
    this->waitAnyEventTimeout(ALL_EVENTS, CAN::UPDATE_PERIOD_MSEC);


  		




  111
  112
  
    
  }


  		




  112
  113
  
    

  		




  113
  114
  
    
  return RDY_OK;


  		












  

    
      devices/DiWheelDrive/DiWheelDrive.cpp
    
  





  148
  148
  
    
  for (int idx = 0; idx < 4; ++idx)


  		




  149
  149
  
    
    this->proximityFloorValue[idx] = global.vcnl4020[idx].getProximityScaledWoOffset();


  		




  150
  150
  
    

  		




  
  151
  
    
  // Update magnetometer values


  		




  
  152
  
    
  for (uint8_t axis = 0; axis < 3; ++axis) {


  		




  
  153
  
    
    this->magnetometerValue[axis] = global.hmc5883l.getMagnetizationGauss(axis);


  		




  
  154
  
    
  }


  		




  
  155
  
    

  		




  
  156
  
    
  // Update gyroscope values


  		




  
  157
  
    
  for (uint8_t axis = 0; axis < 3; ++axis) {


  		




  
  158
  
    
    this->gyroscopeValue[axis] = global.l3g4200d.getAngularRate(axis);


  		




  
  159
  
    
  }


  		




  
  160
  
    

  		




  151
  161
  
    
  return 0;


  		




  152
  162
  
    
}


  		




  153
  163
  
    

  		




  ......




  164
  174
  
    

  		




  165
  175
  
    
  // Send the valocites µm/s of the x axis and µrad/s around z axis: end


  		




  166
  176
  
    
  // Send the odometry: start


  		




  167
  
  
    
  //       BaseThread::sleep(MS2ST(10));  // Sleep, otherwise the cognition-board wont receive all messages


  		




  
  177
  
    
  BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee


  		




  168
  178
  
    
  // Set the frame id


  		




  169
  179
  
    
  frame.SID = 0;


  		




  170
  180
  
    
  this->encodeDeviceId(&frame, CAN::ODOMETRY_ID);


  		




  ......




  181
  191
  
    

  		




  182
  192
  
    
  // Send the odometry: end


  		




  183
  193
  
    
  // Send the proximity values of the floor: start


  		




  184
  
  
    
  //       BaseThread::sleep(MS2ST(10));  // Sleep, otherwise the cognition-board wont receive all messages


  		




  
  194
  
    
  BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee


  		




  185
  195
  
    
  // Set the frame id


  		




  186
  196
  
    
  frame.SID = 0;


  		




  187
  197
  
    
  this->encodeDeviceId(&frame, CAN::PROXIMITY_FLOOR_ID);


  		




  ......




  192
  202
  
    
  frame.DLC = 8;


  		




  193
  203
  
    
  this->transmitMessage(&frame);


  		




  194
  204
  
    

  		




  
  205
  
    
  // Send the magnetometer data


  		




  
  206
  
    
  for (uint8_t axis = 0; axis < 3; ++axis) {


  		




  
  207
  
    
    frame.SID = 0;


  		




  
  208
  
    
    this->encodeDeviceId(&frame, CAN::MAGNETOMETER_X_ID + axis); // Y- and Z-axis have according IDs


  		




  
  209
  
    
    frame.data32[0] = this->magnetometerValue[axis];


  		




  
  210
  
    
    frame.DLC = 4;


  		




  
  211
  
    
    this->transmitMessage(&frame);


  		




  
  212
  
    
  }


  		




  
  213
  
    

  		




  
  214
  
    
  // Send gyroscope data


  		




  
  215
  
    
  frame.SID = 0;


  		




  
  216
  
    
  this->encodeDeviceId(&frame, CAN::GYROSCOPE_ID);


  		




  
  217
  
    
  frame.data16[0] = this->gyroscopeValue[0];


  		




  
  218
  
    
  frame.data16[1] = this->gyroscopeValue[1];


  		




  
  219
  
    
  frame.data16[2] = this->gyroscopeValue[2];


  		




  
  220
  
    
  frame.DLC = 6;


  		




  
  221
  
    
  this->transmitMessage(&frame);


  		




  
  222
  
    

  		




  195
  223
  
    
  // Send the board ID (board ID of DiWheelDrive = Robot ID)


  		




  196
  224
  
    
  if (this->bcCounter % 10 == 0) {


  		




  197
  225
  
    
    frame.SID = 0;


  		












  

    
      devices/DiWheelDrive/global.hpp
    
  





  55
  55
  
    
    /* command             */ VCNL4020::ALS_EN | VCNL4020::PROX_EN | VCNL4020::SELFTIMED_EN,


  		




  56
  56
  
    
    /* ambient parameter   */ VCNL4020::AMBIENT_RATE_2 | VCNL4020::AMBIENT_AUTO_OFFSET | VCNL4020::AMBIENT_AVG_32,


  		




  57
  57
  
    
    /* IR LED current [mA] */ 200u,


  		




  58
  
  
    
    /* proximity rate      */ VCNL4020::PROX_RATE_7_8125


  		




  
  58
  
    
    /* proximity rate      */ VCNL4020::PROX_RATE_125


  		




  59
  59
  
    
  };


  		




  60
  60
  
    

  		




  61
  61
  
    
  /**


  		




  ......




  190
  190
  
    
    increments(&QEID3, &QEID4),


  		




  191
  191
  
    
    motorcontrol(&PWMD2, &increments, GPIOB, GPIOB_POWER_EN, &memory),


  		




  192
  192
  
    
    distcontrol(&motorcontrol, &increments),


  		




  193
  
  
    
    odometry(&increments),


  		




  
  193
  
    
    odometry(&increments, &l3g4200d),


  		




  194
  194
  
    
    sercanmux1(&SD1, &CAND1, CAN::DI_WHEEL_DRIVE_ID),


  		




  195
  195
  
    
    robot(&CAND1),


  		




  196
  196
  
    
    userThread()


  		












  

    
      devices/DiWheelDrive/main.cpp
    
  





  792
  792
  
    
   */


  		




  793
  793
  
    
  BaseThread::sleep(MS2ST(2000));


  		




  794
  794
  
    

  		




  795
  
  
    
  boardClearI2CBus(GPIOB_COMPASS_SCL);


  		




  796
  
  
    
  boardClearI2CBus(GPIOB_IR_SCL);


  		




  
  795
  
    
  boardClearI2CBus(GPIOB_COMPASS_SCL, GPIOB_COMPASS_SDA);


  		




  
  796
  
    
  boardClearI2CBus(GPIOB_IR_SCL, GPIOB_IR_SDA);


  		




  797
  797
  
    

  		




  798
  798
  
    
  global.HW_I2C1.start(&global.i2c1_config);


  		




  799
  799
  
    
  global.HW_I2C2.start(&global.i2c2_config);


  		












  

    
      devices/DiWheelDrive/userthread.cpp
    
  





  76
  76
  
    

  		




  77
  77
  
    
const int blackStartFalling = 0x1000; // Where the black curve starts falling


  		




  78
  78
  
    
const int blackOff = 0x1800; // Where no more black is detected


  		




  79
  
  
    
const int whiteStartRising = 0x4000; // Where the white curve starts rising


  		




  80
  
  
    
const int whiteOn = 0x8000; // Where the white curve has reached the maximum value


  		




  
  79
  
    
const int whiteStartRising = 0x2800; // Where the white curve starts rising


  		




  
  80
  
    
const int whiteOn = 0x6000; // Where the white curve has reached the maximum value


  		




  81
  81
  
    
const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum


  		




  82
  82
  
    
const int greyStartRising = blackStartFalling; // Where grey starts rising


  		




  83
  83
  
    
const int greyOff = whiteOn; // Where grey is completely off again


  		




  ......




  188
  188
  
    
// Get a crisp output for the steering commands


  		




  189
  189
  
    
void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) {


  		




  190
  190
  
    

  		




  191
  
  
    
	if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&


  		




  192
  
  
    
	     member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {


  		




  
  191
  
    
	// all sensors are equal


  		




  
  192
  
    
	if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&


  		




  
  193
  
    
	    member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] &&


  		




  
  194
  
    
	    member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {


  		




  
  195
  
    
		// something is wrong -> stop


  		




  
  196
  
    
		copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);


  		




  
  197
  
    
	// both front sensor detect a line


  		




  
  198
  
    
	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&


  		




  
  199
  
    
	    member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {


  		




  193
  200
  
    
		// straight


  		




  194
  201
  
    
		copyRpmSpeed(rpmForward, rpmFuzzyCtrl);


  		




  
  202
  
    
	// exact one front sensor detects a line


  		




  195
  203
  
    
	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK ||


  		




  196
  204
  
    
	           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {


  		




  197
  205
  
    
		// soft correction


  		




  198
  
  
    
		if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY)


  		




  
  206
  
    
		if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {


  		




  199
  207
  
    
			// soft right


  		




  200
  208
  
    
			copyRpmSpeed(rpmSoftRight, rpmFuzzyCtrl);


  		




  201
  
  
    
		else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE)


  		




  
  209
  
    
		} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) {


  		




  202
  210
  
    
			// hard right


  		




  203
  211
  
    
			copyRpmSpeed(rpmHardRight, rpmFuzzyCtrl);


  		




  204
  
  
    
		else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY)


  		




  
  212
  
    
		} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {


  		




  205
  213
  
    
			// soft left


  		




  206
  214
  
    
			copyRpmSpeed(rpmSoftLeft, rpmFuzzyCtrl);


  		




  207
  
  
    
		else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE)


  		




  
  215
  
    
		} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {


  		




  208
  216
  
    
			// hard left


  		




  209
  217
  
    
			copyRpmSpeed(rpmHardLeft, rpmFuzzyCtrl);


  		




  210
  
  
    
	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY ||


  		




  
  218
  
    
		}


  		




  
  219
  
    
	// both wheel sensors detect a line


  		




  
  220
  
    
	} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK &&


  		




  
  221
  
    
	           member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {


  		




  
  222
  
    
		// something is wrong -> stop


  		




  
  223
  
    
		copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);


  		




  
  224
  
    
	// exactly one wheel sensor detects a line


  		




  
  225
  
    
	} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK ||


  		




  
  226
  
    
	           member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {


  		




  
  227
  
    
		if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK) {


  		




  
  228
  
    
			// turn left


  		




  
  229
  
    
			copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);


  		




  
  230
  
    
		} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {


  		




  
  231
  
    
			// turn right


  		




  
  232
  
    
			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);


  		




  
  233
  
    
		}


  		




  
  234
  
    
	// both front sensors may detect a line


  		




  
  235
  
    
	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY &&


  		




  211
  236
  
    
	           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {


  		




  212
  
  
    
		if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE)


  		




  
  237
  
    
		if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {


  		




  
  238
  
    
			// turn left


  		




  
  239
  
    
			copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);


  		




  
  240
  
    
		} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {


  		




  213
  241
  
    
			// turn right


  		




  214
  242
  
    
			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);


  		




  215
  
  
    
		else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE)


  		




  
  243
  
    
		}


  		




  
  244
  
    
	// exactly one front sensor may detect a line


  		




  
  245
  
    
	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY ||


  		




  
  246
  
    
	           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {


  		




  
  247
  
    
		if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {


  		




  216
  248
  
    
			// turn left


  		




  217
  249
  
    
			copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);


  		




  218
  
  
    
		else


  		




  219
  
  
    
			// go straight


  		




  220
  
  
    
			copyRpmSpeed(rpmForward, rpmFuzzyCtrl);


  		




  221
  
  
    
	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE &&


  		




  222
  
  
    
	           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {


  		




  223
  
  
    
		// go straight and check wheel sensors


  		




  224
  
  
    
		if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] != WHITE)


  		




  
  250
  
    
		} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {


  		




  
  251
  
    
			// turn right


  		




  
  252
  
    
			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);


  		




  
  253
  
    
		}


  		




  
  254
  
    
	// both wheel sensors may detect a line


  		




  
  255
  
    
	} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY &&


  		




  
  256
  
    
	           member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {


  		




  
  257
  
    
		// something is wrong -> stop


  		




  
  258
  
    
		copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);


  		




  
  259
  
    
	// exactly one wheel sensor may detect a line


  		




  
  260
  
    
	} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY ||


  		




  
  261
  
    
	           member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {


  		




  
  262
  
    
		if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {


  		




  225
  263
  
    
			// turn left


  		




  226
  264
  
    
			copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);


  		




  227
  
  
    
		else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] != WHITE)


  		




  
  265
  
    
		} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {


  		




  228
  266
  
    
			// turn right


  		




  229
  267
  
    
			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);


  		




  230
  
  
    
		else


  		




  231
  
  
    
            // line is lost -> stop


  		




  232
  
  
    
            copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);


  		




  
  268
  
    
		}


  		




  
  269
  
    
	// no sensor detects anything 


  		




  
  270
  
    
	} else {


  		




  
  271
  
    
		// line is lost -> stop


  		




  
  272
  
    
		copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);


  		




  233
  273
  
    
	}


  		




  234
  274
  
    

  		




  235
  275
  
    
	return;


  		




  ......




  371
  411
  
    
            setRpmSpeed(rpmFuzzyCtrl);


  		




  372
  412
  
    
        }


  		




  373
  413
  
    

  		




  374
  
  
    
		this->sleep(MS2ST(100));


  		




  
  414
  
    
		this->sleep(MS2ST(10));


  		




  375
  415
  
    
	}


  		




  376
  416
  
    

  		




  377
  417
  
    
  return RDY_OK;


  		












  

    
      devices/LightRing/main.cpp
    
  





  514
  514
  
    

  		




  515
  515
  
    
  extStart(&EXTD1, &extcfg);


  		




  516
  516
  
    

  		




  517
  
  
    
  boardClearI2CBus(GPIOB_MEM_SCL);


  		




  
  517
  
    
  boardClearI2CBus(GPIOB_MEM_SCL, GPIOB_MEM_SDA);


  		




  518
  518
  
    

  		




  519
  519
  
    
  global.HW_I2C2.start(&global.i2c2_config);


  		




  520
  520
  
    

  		












  

    
      devices/PowerManagement/PowerManagement.cpp
    
  





  133
  133
  
    
    frame.data16[0] = this->proximityRingValue[i];


  		




  134
  134
  
    
    frame.DLC = 2;


  		




  135
  135
  
    
    this->transmitMessage(&frame);


  		




  136
  
  
    
    // HACK This is a first workaround (do wrapping of 4 sensors, better timing than 10 ms in sleep)


  		




  137
  
  
    
    // I choosed 10 ms, because 10 ms x 8 = 80 ms < 125 ms which is the updaterate CAN::UPDATE_PERIOD_MSEC


  		




  138
  
  
    
    BaseThread::sleep(MS2ST(10));  // Sleep, otherwise the cognition-board wont receive all messages


  		




  
  136
  
    
    BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee


  		




  139
  137
  
    
  }


  		




  140
  138
  
    
  ++this->bc_counter;


  		




  141
  139
  
    
}


  		












  

    
      devices/PowerManagement/global.hpp
    
  





  55
  55
  
    

  		




  56
  56
  
    
  I2CConfig i2c1_config{


  		




  57
  57
  
    
    /* I2C mode                 */ OPMODE_I2C,


  		




  58
  
  
    
    /* frequency                */ 200000,


  		




  59
  
  
    
    /* I2C fast mode duty cycle */ FAST_DUTY_CYCLE_2


  		




  
  58
  
    
    /* frequency                */ 100000,


  		




  
  59
  
    
    /* I2C fast mode duty cycle */ STD_DUTY_CYCLE


  		




  60
  60
  
    
  };


  		




  61
  61
  
    
  I2CConfig i2c2_config{


  		




  62
  62
  
    
    /* I2C mode                 */ OPMODE_I2C,


  		




  63
  
  
    
    /* frequency                */ 200000,


  		




  64
  
  
    
    /* I2C fast mode duty cycle */ FAST_DUTY_CYCLE_2


  		




  
  63
  
    
    /* frequency                */ 100000,


  		




  
  64
  
    
    /* I2C fast mode duty cycle */ STD_DUTY_CYCLE


  		




  65
  65
  
    
  };


  		




  66
  66
  
    

  		




  67
  67
  
    
  VCNL4020::VCNL4020Config vcnl4020_config{


  		




  68
  68
  
    
    /* command             */ VCNL4020::ALS_EN | VCNL4020::PROX_EN | VCNL4020::SELFTIMED_EN,


  		




  69
  69
  
    
    /* ambient parameter   */ VCNL4020::AMBIENT_RATE_2 | VCNL4020::AMBIENT_AUTO_OFFSET | VCNL4020::AMBIENT_AVG_32,


  		




  70
  70
  
    
    /* IR LED current [mA] */ 200u,


  		




  71
  
  
    
    /* proximity rate      */ VCNL4020::PROX_RATE_7_8125


  		




  
  71
  
    
    /* proximity rate      */ VCNL4020::PROX_RATE_125


  		




  72
  72
  
    
  };


  		




  73
  73
  
    

  		




  74
  74
  
    
  adcsample_t adc1_buffer[1];


  		












  

    
      devices/PowerManagement/main.cpp
    
  





  976
  976
  
    

  		




  977
  977
  
    
  extStart(&EXTD1, &extcfg);


  		




  978
  978
  
    

  		




  979
  
  
    
  boardClearI2CBus(GPIOB_GAUGE_SCL1);


  		




  980
  
  
    
  boardClearI2CBus(GPIOB_GAUGE_SCL2);


  		




  
  979
  
    
  boardClearI2CBus(GPIOB_GAUGE_SCL1, GPIOB_GAUGE_SDA1);


  		




  
  980
  
    
  boardClearI2CBus(GPIOB_GAUGE_SCL2, GPIOB_GAUGE_SDA2);


  		




  981
  981
  
    

  		




  982
  982
  
    
  global.HW_I2C1.start(&global.i2c1_config);


  		




  983
  983
  
    
  global.HW_I2C2.start(&global.i2c2_config);


  		




  984
  984
  
    

  		




  
  985
  
    
  uint16_t i2c_test = 0;


  		




  
  986
  
    
  while (global.ina219[INA_VIO18].readRegister(INA219::Driver::REG_BUS_VOLTAGE, i2c_test) != RDY_OK) {


  		




  
  987
  
    
    chprintf((BaseSequentialStream*)&global.sercanmux1, "I2C #1 stalled! trying to clear the bus... (this will take 20 seconds)\n");


  		




  
  988
  
    
    boardWriteLed(1);


  		




  
  989
  
    
    boardResetBQ27500I2C(GPIOB_GAUGE_SCL2, GPIOB_GAUGE_SDA2);


  		




  
  990
  
    
    boardWriteLed(0);


  		




  
  991
  
    
  }


  		




  
  992
  
    
  while (global.ina219[INA_VDD].readRegister(INA219::Driver::REG_BUS_VOLTAGE, i2c_test) != RDY_OK) {


  		




  
  993
  
    
    chprintf((BaseSequentialStream*)&global.sercanmux1, "I2C #2 stalled! trying to clear the bus... (this will take 20 seconds)\n");


  		




  
  994
  
    
    boardWriteLed(1);


  		




  
  995
  
    
    boardResetBQ27500I2C(GPIOB_GAUGE_SCL1, GPIOB_GAUGE_SDA1);


  		




  
  996
  
    
    boardWriteLed(0);


  		




  
  997
  
    
  }


  		




  
  998
  
    

  		




  985
  999
  
    
  global.memory.init();


  		




  986
  1000
  
    
  uint8_t i = 0;


  		




  987
  1001
  
    
  if (global.memory.getBoardId(&i) == fileSystemIo::FileSystemIoBase::OK) {


  		












  

    
      devices/PowerManagement/userthread.cpp
    
  





  14
  14
  
    
bool running;


  		




  15
  15
  
    

  		




  16
  16
  
    
uint16_t constexpr proxThresholdLow = 0x0000;


  		




  17
  
  
    
uint16_t constexpr proxThresholdHigh = 0x2000;


  		




  
  17
  
    
uint16_t constexpr proxThresholdHigh = 0x1000;


  		




  18
  18
  
    
uint16_t constexpr proxRange = proxThresholdHigh - proxThresholdLow;


  		




  19
  19
  
    

  		




  20
  20
  
    
std::array< std::array<float, 2>, 8> constexpr namMatrix = {


  		




  ......




  28
  28
  
    
    std::array<float, 2>/* ESE */{ 0.25f,  0.25f},


  		




  29
  29
  
    
    std::array<float, 2>/* SSE */{ 0.00f,  0.00f}


  		




  30
  30
  
    
};


  		




  31
  
  
    
uint32_t constexpr baseTranslation = 50e3; // 2cm/s


  		




  
  31
  
    
uint32_t constexpr baseTranslation = 100e3; // 2cm/s


  		




  32
  32
  
    
uint32_t constexpr baseRotation = 1e6; // 1rad/s


  		




  33
  33
  
    
types::kinematic constexpr defaultKinematic = {


  		




  34
  34
  
    
    /*  x  [µm/s]   */ baseTranslation,


  		




  ......




  161
  161
  
    
            global.robot.setTargetSpeed(kinematic);


  		




  162
  162
  
    
        }


  		




  163
  163
  
    

  		




  164
  
  
    
        this->sleep(MS2ST(100));


  		




  
  164
  
    
        this->sleep(MS2ST(10));


  		




  165
  165
  
    
    }


  		




  166
  166
  
    

  		




  167
  167
  
    
  return RDY_OK;


  		












  

    
      devices/flash.mk
    
  





  4
  4
  
    
PowerManagement := 2


  		




  5
  5
  
    
LightRing := 3


  		




  6
  6
  
    

  		




  7
  
  
    
SERIALBOOT ?= SerialBoot


  		




  
  7
  
    
SERIALBOOT ?= $(dir $(abspath $(lastword $(MAKEFILE_LIST))))../../amiro-blt/Host/Source/SerialBoot/build/SerialBoot


  		




  
  8
  
    
$(info $(SERIALBOOT))


  		




  
  9
  
    

  		




  8
  10
  
    
ifeq ($(OS),Windows_NT)


  		




  9
  11
  
    
	SERIALBOOT_PORT ?= COM4


  		




  10
  12
  
    
else


  		




  ......




  17
  19
  
    
#SERIALBOOT_BT_ADDR ?= 00:00:00:00:00:00


  		




  18
  20
  
    
SERIALBOOT_BT_ADDR ?= 00:07:80:44:23:F9


  		




  19
  21
  
    

  		




  20
  
  
    

  		




  21
  22
  
    
ifdef PROJECT


  		




  22
  23
  
    
	DEVICES :=


  		




  23
  24
  
    
	FLASHFILE = build/$(PROJECT).srec


  		












  

    
      include/amiro/Constants.h
    
  





  37
  37
  
    

  		




  38
  38
  
    
namespace CAN {


  		




  39
  39
  
    

  		




  40
  
  
    
  const uint32_t UPDATE_PERIOD_MSEC        = MS2ST(125);


  		




  
  40
  
    
  const uint32_t UPDATE_PERIOD_MSEC        = US2ST(62500);  // 16 Hz


  		




  41
  41
  
    

  		




  42
  42
  
    
  const uint32_t PERIODIC_TIMER_ID         = 1;


  		




  43
  43
  
    
  const uint32_t RECEIVED_ID               = 2;


  		




  ......




  54
  54
  
    
  const uint32_t LIGHT_RING_ID             = 3;


  		




  55
  55
  
    
  const uint32_t COGNITION                 = 4;


  		




  56
  56
  
    

  		




  57
  
  
    

  		




  
  57
  
    
  const uint32_t MAGNETOMETER_X_ID         = 0x54;


  		




  
  58
  
    
  const uint32_t MAGNETOMETER_Y_ID         = 0x55;


  		




  
  59
  
    
  const uint32_t MAGNETOMETER_Z_ID         = 0x56;


  		




  
  60
  
    
  const uint32_t GYROSCOPE_ID              = 0x58;


  		




  58
  61
  
    
  const uint32_t PROXIMITY_FLOOR_ID        = 0x51;


  		




  59
  62
  
    
  const uint32_t ODOMETRY_ID               = 0x50;


  		




  60
  63
  
    
  const uint32_t BRIGHTNESS_ID             = 0x40;


  		












  

    
      include/amiro/ControllerAreaNetworkRx.h
    
  





  20
  20
  
    
    types::position getOdometry();


  		




  21
  21
  
    
    types::power_status& getPowerStatus();


  		




  22
  22
  
    
    uint8_t getRobotID();


  		




  
  23
  
    
    int32_t getMagnetometerValue(int axis);


  		




  
  24
  
    
    int16_t getGyroscopeValue(int axis);


  		




  23
  25
  
    

  		




  24
  26
  
    
    void calibrateProximityRingValues();


  		




  25
  27
  
    
    void calibrateProximityFloorValues();


  		




  ......




  38
  40
  
    
    uint16_t proximityRingValue[8];


  		




  39
  41
  
    
    int actualSpeed[2];


  		




  40
  42
  
    
    uint16_t proximityFloorValue[4];


  		




  
  43
  
    
    int32_t magnetometerValue[3];


  		




  
  44
  
    
    int16_t gyroscopeValue[3];


  		




  41
  45
  
    
    types::position robotPosition;


  		




  42
  46
  
    
    types::power_status powerStatus;


  		




  43
  47
  
    
    uint8_t robotId;


  		












  

    
      include/amiro/Odometry.h
    
  





  2
  2
  
    
#define AMIRO_ODOMETRY_H_


  		




  3
  3
  
    

  		




  4
  4
  
    
#include <amiro/MotorControl.h>


  		




  
  5
  
    
#include <amiro/gyro/l3g4200d.hpp>


  		




  5
  6
  
    

  		




  6
  7
  
    
#include <Types.h> // types::position


  		




  7
  8
  
    

  		




  ......




  13
  14
  
    
     * Constructor


  		




  14
  15
  
    
     *


  		




  15
  16
  
    
     * @param mi object for retrieving the motor increments of the qei


  		




  
  17
  
    
     * @param gyro object for retrieving the gyroscope data


  		




  
  18
  
    
     * @param mc object for retrieving calibration parameters


  		




  16
  19
  
    
     */


  		




  17
  
  
    
    Odometry(MotorIncrements* mi);


  		




  
  20
  
    
    Odometry(MotorIncrements* mi, L3G4200D* gyroscope);


  		




  18
  21
  
    

  		




  19
  22
  
    
    /**


  		




  20
  23
  
    
     * Set the position of the roboter


  		




  ......




  74
  77
  
    
     */


  		




  75
  78
  
    
    void updateOdometry();


  		




  76
  79
  
    

  		




  77
  
  
    
    MotorIncrements* motorIncrements;


  		




  
  80
  
    
    MotorIncrements* motorIncrements; // QEI driver


  		




  
  81
  
    
    L3G4200D* gyro;  // Gyroscope driver


  		




  78
  82
  
    
    chibios_rt::EvtSource eventSource;


  		




  79
  83
  
    
    const unsigned int period;


  		




  80
  84
  
    
    int incrementsPerRevolution;


  		












  

    
      include/amiro/gyro/l3g4200d.hpp
    
  





  15
  15
  
    
    DR_200_HZ   = 0x40,


  		




  16
  16
  
    
    DR_400_HZ   = 0x80,


  		




  17
  17
  
    
    DR_800_HZ   = 0xC0,


  		




  
  18
  
    
    DR_MASK     = 0xC0,


  		




  18
  19
  
    
  };


  		




  19
  20
  
    
  enum {


  		




  20
  21
  
    
    BW_12_5  = 0x00,


  		




  ......




  25
  26
  
    
    BW_50    = 0x20,


  		




  26
  27
  
    
    BW_70    = 0x30,


  		




  27
  28
  
    
    BW_110   = 0x30,


  		




  
  29
  
    
    BW_MASK  = 0x30,


  		




  28
  30
  
    
  };


  		




  29
  31
  
    
  enum {


  		




  30
  
  
    
    PD  = 0x08,


  		




  31
  
  
    
    ZEN = 0x04,


  		




  32
  
  
    
    YEN = 0x02,


  		




  33
  
  
    
    XEN = 0x01,


  		




  
  32
  
    
    PD       = 0x08,


  		




  
  33
  
    
    ZEN      = 0x04,


  		




  
  34
  
    
    YEN      = 0x02,


  		




  
  35
  
    
    XEN      = 0x01,


  		




  
  36
  
    
    EN_MASK  = 0x0F,


  		




  34
  37
  
    
  };


  		




  35
  38
  
    
  enum {


  		




  36
  39
  
    
    HPM_NORMAL_RST = 0x00,


  		




  ......




  73
  76
  
    
    ST_EN       = 0x02,


  		




  74
  77
  
    
    SIM_3W      = 0x01,


  		




  75
  78
  
    
    SIM_4W      = 0x00,


  		




  
  79
  
    
    FS_MASK     = 0x20,


  		




  76
  80
  
    
  };


  		




  77
  81
  
    
  enum {


  		




  78
  82
  
    
    BOOT          = 0x80,


  		




  ......




  224
  228
  
    
  chibios_rt::EvtSource* getEventSource();


  		




  225
  229
  
    
  msg_t configure(const L3G4200DConfig* config);


  		




  226
  230
  
    
  int16_t angularRate[AXIS_Z - AXIS_X + 1];


  		




  
  231
  
    
  int32_t angular[AXIS_Z - AXIS_X + 1];


  		




  227
  232
  
    

  		




  228
  233
  
    
  int16_t getAngularRate(const uint8_t axis);


  		




  
  234
  
    
  


  		




  
  235
  
    
  int32_t getAngularRate_udps(const uint8_t axis);


  		




  
  236
  
    
  int32_t getAngular(const uint8_t axis);


  		




  
  237
  
    
  int32_t getAngular_ud(const uint8_t axis);


  		




  
  238
  
    
  void angularReset();


  		




  229
  239
  
    

  		




  230
  240
  
    

  		




  231
  241
  
    
  /**


  		




  ......




  240
  250
  
    

  		




  241
  251
  
    
 private:


  		




  242
  252
  
    
  inline void updateSensorData();


  		




  
  253
  
    
  inline void calcAngular();


  		




  243
  254
  
    

  		




  244
  255
  
    
 private:


  		




  245
  256
  
    

  		




  246
  257
  
    
  HWSPIDriver* driver;


  		




  247
  258
  
    
  chibios_rt::EvtSource eventSource;


  		




  
  259
  
    
  uint32_t integrationTic;


  		




  
  260
  
    
  uint32_t udpsPerTic; // Resolution: Micro-degree-per-second per digit


  		




  
  261
  
    
  uint32_t period_us;


  		




  
  262
  
    
  uint32_t period_ms;


  		




  
  263
  
    
  systime_t period_st;


  		




  248
  264
  
    

  		




  249
  265
  
    
};


  		




  250
  266
  
    

  		












  

    
      include/amiro/power/ina219.hpp
    
  





  140
  140
  
    
        Status() {bus_voltage.value = 0; power = 0;}


  		




  141
  141
  
    
      };


  		




  142
  142
  
    

  		




  
  143
  
    
    public:


  		




  143
  144
  
    
      enum RegisterAddress {


  		




  144
  145
  
    
        REG_CONFIGURATION = 0x00u,


  		




  145
  146
  
    
        REG_SHUNT_VOLTAGE = 0x01u,


  		




  ......




  149
  150
  
    
        REG_CALIBRATION = 0x05u


  		




  150
  151
  
    
      };


  		




  151
  152
  
    

  		




  
  153
  
    
    private:


  		




  152
  154
  
    
      enum RegisterMask {


  		




  153
  155
  
    
        MASK_CONFIGURATION = 0x3FFFu,


  		




  154
  156
  
    
        MASK_CALIBRATION = 0xFFFEu,


  		




  ......




  204
  206
  
    

  		




  205
  207
  
    
      uint8_t reset();


  		




  206
  208
  
    

  		




  207
  
  
    
  protected:


  		




  
  209
  
    
    protected:


  		




  208
  210
  
    
      virtual msg_t main(void);


  		




  209
  211
  
    

  		




  210
  
  
    
    private:


  		




  
  212
  
    
    public:


  		




  211
  213
  
    
      msg_t readRegister(const RegisterAddress reg, uint16_t& dst);


  		




  212
  214
  
    
      msg_t writeRegister(const RegisterAddress reg, const uint16_t& val);


  		




  213
  215
  
    

  		




  
  216
  
    
    private:


  		




  214
  217
  
    
      static inline INA219::BusVoltage busVoltageReg2uV(const INA219::Driver::BusVoltage reg_val)


  		




  215
  218
  
    
      {


  		




  216
  219
  
    
        INA219::BusVoltage bus_voltage;


  		












Also available in:  Unified diff