Thomas Hartmann / HoverboardTest

Simple piece of code for simulation sensor inputs of a hoverboard to do basic control of wheels.

Dependencies:   WiiNunchuck mbed-dev

This program is inspired by this site:

http://drewspewsmuse.blogspot.co.at/2016/06/how-i-hacked-self-balancing-scooter.html

Here you can get more information of the protocol revervse engineering.

This program uses a Wii nunchuk connected over I2C to controll the speed. Speed is controlled by a closed loop P controller. Feedback is done by counting HALL events from the brushless motor. Some more pictures are available here:

https://github.com/tyler123durden/hoverboard-hardware

Basically you need a serial TX and 3 HALL IRQs per motor side.

Wiring of the board. Gyro boards are not used, input is generated from nucleo board. Motor board has still the HALL connections. Nucleo board just also reads HALL inputs to get speed information for closed loop controll.

/media/uploads/Thomas_H/wiring.jpg

main.cpp@2:6e0dfe2caf48, 2016-10-23 (annotated)

Committer:
Thomas_H
Date:
Sun Oct 23 19:38:35 2016 +0000
Revision:
2:6e0dfe2caf48
Parent:
1:5acd27cb1857 
removed unused files

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Thomas_H 0:fe156910301e 1 #include "mbed.h"
Thomas_H 0:fe156910301e 2 #include "HallDecoder.h"
Thomas_H 0:fe156910301e 3 #include "WiiNunchuck.h"
Thomas_H 0:fe156910301e 4
Thomas_H 0:fe156910301e 5 //------------------------------------
Thomas_H 0:fe156910301e 6 // Hyperterminal configuration
Thomas_H 0:fe156910301e 7 // 9600 bauds, 8-bit data, no parity
Thomas_H 0:fe156910301e 8 //------------------------------------
Thomas_H 0:fe156910301e 9
Thomas_H 1:5acd27cb1857 10 #define NELEMS(x) (sizeof(x) / sizeof(x[0])) // number of elements in array
Thomas_H 0:fe156910301e 11
Thomas_H 1:5acd27cb1857 12 #define DEBUG_SERIAL_SPEED 115200
Thomas_H 0:fe156910301e 13
Thomas_H 1:5acd27cb1857 14 #define NUNCHUCK_I2C_CLK PA_15
Thomas_H 1:5acd27cb1857 15 #define NUNCHUCK_I2C_SDA PA_14
Thomas_H 0:fe156910301e 16 #define NUNCHUCK_X_OFFSET 123
Thomas_H 0:fe156910301e 17 #define NUNCHUCK_Y_OFFSET 127
Thomas_H 0:fe156910301e 18
Thomas_H 1:5acd27cb1857 19 #define MOTOR_LEFT_TXD PB_10 // SERIAL1
Thomas_H 1:5acd27cb1857 20 #define MOTOR_LEFT_RXD NC
Thomas_H 1:5acd27cb1857 21 #define MOTOR_LEFT_HALL1 PB_15
Thomas_H 1:5acd27cb1857 22 #define MOTOR_LEFT_HALL2 PB_14
Thomas_H 1:5acd27cb1857 23 #define MOTOR_LEFT_HALL3 PB_6
Thomas_H 1:5acd27cb1857 24
Thomas_H 1:5acd27cb1857 25 #define MOTOR_RIGHT_TXD PC_4 // SERIAL3
Thomas_H 0:fe156910301e 26 #define MOTOR_RIGHT_RXD NC
Thomas_H 1:5acd27cb1857 27 #define MOTOR_RIGHT_HALL1 PC_7
Thomas_H 1:5acd27cb1857 28 #define MOTOR_RIGHT_HALL2 PA_8
Thomas_H 1:5acd27cb1857 29 #define MOTOR_RIGHT_HALL3 PA_9
Thomas_H 1:5acd27cb1857 30
Thomas_H 1:5acd27cb1857 31 #define HOVER_SPEED_MAX (90*2) // ticks per second
Thomas_H 1:5acd27cb1857 32
Thomas_H 1:5acd27cb1857 33 /******************************************************************************/
Thomas_H 1:5acd27cb1857 34 WiiNunchuck m_oNunchuck(NUNCHUCK_I2C_SDA, NUNCHUCK_I2C_CLK); // I2C1
Thomas_H 1:5acd27cb1857 35 Serial m_oPc(SERIAL_TX, SERIAL_RX); // PA_2, PA_3 -> SERIAL2
Thomas_H 1:5acd27cb1857 36 Timer m_oTimer;
Thomas_H 1:5acd27cb1857 37 Ticker m_oPeriodic;
Thomas_H 1:5acd27cb1857 38 DigitalOut led1(LED1);
Thomas_H 1:5acd27cb1857 39
Thomas_H 1:5acd27cb1857 40 HallDecoder oHallLeft(MOTOR_LEFT_HALL1, MOTOR_LEFT_HALL2, MOTOR_LEFT_HALL3);
Thomas_H 1:5acd27cb1857 41 HallDecoder oHallRight(MOTOR_RIGHT_HALL1, MOTOR_RIGHT_HALL2, MOTOR_RIGHT_HALL3);
Thomas_H 1:5acd27cb1857 42
Thomas_H 1:5acd27cb1857 43 typedef struct t_controller {
Thomas_H 1:5acd27cb1857 44 int16_t w; //< setpoint
Thomas_H 1:5acd27cb1857 45 int16_t u; //< input for controler
Thomas_H 1:5acd27cb1857 46 int16_t y; //< actual value
Thomas_H 1:5acd27cb1857 47 int16_t yfilt[3];
Thomas_H 1:5acd27cb1857 48 int16_t e; //< error
Thomas_H 1:5acd27cb1857 49 int16_t eSum; //< error sum
Thomas_H 1:5acd27cb1857 50 float kp; //< controller setting
Thomas_H 1:5acd27cb1857 51 float ki; //< controller setting
Thomas_H 1:5acd27cb1857 52 float kd; //< controller setting
Thomas_H 1:5acd27cb1857 53 } t_controller;
Thomas_H 1:5acd27cb1857 54
Thomas_H 1:5acd27cb1857 55 #define LEFT 0
Thomas_H 1:5acd27cb1857 56 #define RIGHT 1
Thomas_H 1:5acd27cb1857 57 volatile t_controller _Controlloop[2];
Thomas_H 1:5acd27cb1857 58
Thomas_H 1:5acd27cb1857 59
Thomas_H 1:5acd27cb1857 60 #define MAX_ERROR 100
Thomas_H 0:fe156910301e 61
Thomas_H 1:5acd27cb1857 62 /******************************************************************************/
Thomas_H 1:5acd27cb1857 63 // periodic timed control stuff
Thomas_H 1:5acd27cb1857 64 void ticker()
Thomas_H 1:5acd27cb1857 65 {
Thomas_H 1:5acd27cb1857 66 // get new inputs
Thomas_H 1:5acd27cb1857 67 int nTicks[2];
Thomas_H 1:5acd27cb1857 68 nTicks[LEFT] = oHallLeft.getticks();
Thomas_H 1:5acd27cb1857 69 nTicks[RIGHT] = oHallRight.getticks();
Thomas_H 1:5acd27cb1857 70
Thomas_H 1:5acd27cb1857 71 oHallLeft.resetticks();
Thomas_H 1:5acd27cb1857 72 oHallRight.resetticks();
Thomas_H 1:5acd27cb1857 73
Thomas_H 1:5acd27cb1857 74 // for both sides
Thomas_H 1:5acd27cb1857 75 for(int i=0; i<NELEMS(_Controlloop); i++) {
Thomas_H 1:5acd27cb1857 76 _Controlloop[i].yfilt[0] = _Controlloop[i].yfilt[1];
Thomas_H 1:5acd27cb1857 77 _Controlloop[i].yfilt[1] = _Controlloop[i].yfilt[2];
Thomas_H 1:5acd27cb1857 78 _Controlloop[i].yfilt[2] = nTicks[i];
Thomas_H 1:5acd27cb1857 79 _Controlloop[i].y = (_Controlloop[i].yfilt[0]+_Controlloop[i].yfilt[1]+_Controlloop[i].yfilt[2])/3; // low pass filter
Thomas_H 1:5acd27cb1857 80
Thomas_H 1:5acd27cb1857 81 // some max value clipping
Thomas_H 1:5acd27cb1857 82 if(abs(_Controlloop[i].y)>500) {
Thomas_H 1:5acd27cb1857 83 _Controlloop[i].y = 0;
Thomas_H 1:5acd27cb1857 84 }
Thomas_H 1:5acd27cb1857 85
Thomas_H 1:5acd27cb1857 86 int16_t nError = _Controlloop[i].w - _Controlloop[i].y;
Thomas_H 1:5acd27cb1857 87 if(nError>MAX_ERROR) {
Thomas_H 1:5acd27cb1857 88 nError = MAX_ERROR;
Thomas_H 1:5acd27cb1857 89 } else if(_Controlloop[i].w - _Controlloop[i].y<-MAX_ERROR) {
Thomas_H 1:5acd27cb1857 90 nError = -MAX_ERROR;
Thomas_H 1:5acd27cb1857 91 }
Thomas_H 1:5acd27cb1857 92
Thomas_H 1:5acd27cb1857 93 // actual controller equation part
Thomas_H 1:5acd27cb1857 94 _Controlloop[i].eSum += nError;
Thomas_H 1:5acd27cb1857 95 if(_Controlloop[i].eSum>100 ) {
Thomas_H 1:5acd27cb1857 96 _Controlloop[i].eSum = 100;
Thomas_H 1:5acd27cb1857 97 }
Thomas_H 1:5acd27cb1857 98 else if( _Controlloop[i].eSum<-100) {
Thomas_H 1:5acd27cb1857 99 _Controlloop[i].eSum = -100;
Thomas_H 1:5acd27cb1857 100 }
Thomas_H 1:5acd27cb1857 101
Thomas_H 1:5acd27cb1857 102 _Controlloop[i].u = nError* _Controlloop[i].kp + _Controlloop[i].eSum * _Controlloop[i].ki;
Thomas_H 1:5acd27cb1857 103
Thomas_H 1:5acd27cb1857 104 _Controlloop[i].e = nError;
Thomas_H 1:5acd27cb1857 105 }
Thomas_H 1:5acd27cb1857 106
Thomas_H 1:5acd27cb1857 107 // reverse one wheel direction to get same rotation direction for positive inputs
Thomas_H 1:5acd27cb1857 108 _Controlloop[LEFT].u = _Controlloop[LEFT].u * -1;
Thomas_H 0:fe156910301e 109
Thomas_H 1:5acd27cb1857 110 led1 = !led1;
Thomas_H 1:5acd27cb1857 111 }
Thomas_H 0:fe156910301e 112
Thomas_H 1:5acd27cb1857 113 /******************************************************************************/
Thomas_H 0:fe156910301e 114 int main()
Thomas_H 0:fe156910301e 115 {
Thomas_H 1:5acd27cb1857 116 m_oPc.baud(DEBUG_SERIAL_SPEED);
Thomas_H 1:5acd27cb1857 117
Thomas_H 1:5acd27cb1857 118 m_oTimer.start();
Thomas_H 1:5acd27cb1857 119 m_oPeriodic.attach(ticker, 0.5);
Thomas_H 1:5acd27cb1857 120
Thomas_H 1:5acd27cb1857 121 RawSerial oMotorRight(MOTOR_RIGHT_TXD, NC);
Thomas_H 0:fe156910301e 122 oMotorRight.baud(26315);
Thomas_H 0:fe156910301e 123 oMotorRight.format(9, SerialBase::None, 1);
Thomas_H 1:5acd27cb1857 124
Thomas_H 1:5acd27cb1857 125 RawSerial oMotorLeft(MOTOR_LEFT_TXD, NC);
Thomas_H 0:fe156910301e 126 oMotorLeft.baud(26315);
Thomas_H 0:fe156910301e 127 oMotorLeft.format(9, SerialBase::None, 1);
Thomas_H 0:fe156910301e 128
Thomas_H 1:5acd27cb1857 129 int nControl = 1;
Thomas_H 0:fe156910301e 130
Thomas_H 1:5acd27cb1857 131 for(int i=0; i<NELEMS(_Controlloop); i++) {
Thomas_H 1:5acd27cb1857 132 _Controlloop[i].kp = 2.0;
Thomas_H 1:5acd27cb1857 133 _Controlloop[i].ki = 0.3;
Thomas_H 1:5acd27cb1857 134 _Controlloop[i].kd = 0;
Thomas_H 1:5acd27cb1857 135 }
Thomas_H 0:fe156910301e 136
Thomas_H 1:5acd27cb1857 137 m_oPc.printf("Hello Hoverboard !\n");
Thomas_H 0:fe156910301e 138
Thomas_H 0:fe156910301e 139 wait(1);
Thomas_H 0:fe156910301e 140 m_oPc.printf("SystemCoreClock = %d Hz\n", SystemCoreClock);
Thomas_H 0:fe156910301e 141
Thomas_H 0:fe156910301e 142 char c = ' ';
Thomas_H 0:fe156910301e 143
Thomas_H 0:fe156910301e 144 while(1) {
Thomas_H 0:fe156910301e 145 m_oPc.printf("%d\n", c);
Thomas_H 0:fe156910301e 146 if(c == ' ') {
Thomas_H 1:5acd27cb1857 147 _Controlloop[LEFT].w =0;
Thomas_H 0:fe156910301e 148 } else if(c == 'q') {
Thomas_H 1:5acd27cb1857 149 _Controlloop[LEFT].w -= 10;
Thomas_H 0:fe156910301e 150 } else if(c == 'w') {
Thomas_H 1:5acd27cb1857 151 _Controlloop[LEFT].w += 10;
Thomas_H 0:fe156910301e 152 } else if(c == '2') {
Thomas_H 1:5acd27cb1857 153 _Controlloop[LEFT].w += 100;
Thomas_H 0:fe156910301e 154 } else if(c == '1') {
Thomas_H 1:5acd27cb1857 155 _Controlloop[LEFT].w-= 100;
Thomas_H 0:fe156910301e 156 } else if(c=='r') {
Thomas_H 1:5acd27cb1857 157 m_oPc.printf("Reset ticks\n");
Thomas_H 1:5acd27cb1857 158 oHallLeft.resetticks();
Thomas_H 1:5acd27cb1857 159 oHallRight.resetticks();
Thomas_H 0:fe156910301e 160 } else if(c=='n') {
Thomas_H 0:fe156910301e 161 if(nControl==0) {
Thomas_H 0:fe156910301e 162 m_oPc.printf("Change to nunchuck control");
Thomas_H 0:fe156910301e 163 nControl = 1;
Thomas_H 0:fe156910301e 164 } else {
Thomas_H 1:5acd27cb1857 165 m_oPc.printf("Change to keyboard control");
Thomas_H 1:5acd27cb1857 166 nControl = 0;
Thomas_H 0:fe156910301e 167 }
Thomas_H 0:fe156910301e 168 } else if(c=='m') {
Thomas_H 0:fe156910301e 169 m_oPc.printf("nunchuck x: %d\n", m_oNunchuck.joyx());
Thomas_H 0:fe156910301e 170 m_oPc.printf("nunchuck x: %d\n", m_oNunchuck.joyy());
Thomas_H 0:fe156910301e 171 m_oPc.printf("nunchuck c: %d\n", m_oNunchuck.buttonc());
Thomas_H 0:fe156910301e 172 m_oPc.printf("nunchuck z: %d\n", m_oNunchuck.buttonz());
Thomas_H 0:fe156910301e 173 }
Thomas_H 0:fe156910301e 174
Thomas_H 1:5acd27cb1857 175 m_oPc.printf("speed %d\n", _Controlloop[LEFT].w);
Thomas_H 0:fe156910301e 176
Thomas_H 1:5acd27cb1857 177 // main control loop
Thomas_H 0:fe156910301e 178 do {
Thomas_H 1:5acd27cb1857 179 if(m_oTimer.read_ms() > 500) {
Thomas_H 1:5acd27cb1857 180 m_oTimer.reset();
Thomas_H 1:5acd27cb1857 181 m_oPc.printf("Speed set %d speed: %d speed out: %d error: %d \n",
Thomas_H 1:5acd27cb1857 182 _Controlloop[LEFT].w, _Controlloop[LEFT].y, _Controlloop[LEFT].u, _Controlloop[LEFT].e);
Thomas_H 1:5acd27cb1857 183 m_oPc.printf(" Speed set %d speed: %d speed out: %d error: %d \n",
Thomas_H 1:5acd27cb1857 184 _Controlloop[RIGHT].w, _Controlloop[RIGHT].y, _Controlloop[RIGHT].u, _Controlloop[RIGHT].e);
Thomas_H 1:5acd27cb1857 185 }
Thomas_H 0:fe156910301e 186
Thomas_H 0:fe156910301e 187 if(nControl) {
Thomas_H 1:5acd27cb1857 188 int nJoyY = ((m_oNunchuck.joyy()-NUNCHUCK_Y_OFFSET)*HOVER_SPEED_MAX) / 127; // + -> forward
Thomas_H 1:5acd27cb1857 189 int nJoyX = ((m_oNunchuck.joyx()-NUNCHUCK_X_OFFSET)*HOVER_SPEED_MAX) / 127; // + -> right
Thomas_H 1:5acd27cb1857 190
Thomas_H 1:5acd27cb1857 191 if(m_oNunchuck.buttonz()) {
Thomas_H 1:5acd27cb1857 192 // same inputs for both wheels
Thomas_H 1:5acd27cb1857 193 _Controlloop[LEFT].w = nJoyY;
Thomas_H 1:5acd27cb1857 194 _Controlloop[RIGHT].w = nJoyY;
Thomas_H 0:fe156910301e 195 }
Thomas_H 1:5acd27cb1857 196 else {
Thomas_H 1:5acd27cb1857 197 _Controlloop[LEFT].w = nJoyY + nJoyX/2;
Thomas_H 1:5acd27cb1857 198 _Controlloop[RIGHT].w = nJoyY - nJoyX/2;
Thomas_H 1:5acd27cb1857 199 }
Thomas_H 1:5acd27cb1857 200
Thomas_H 1:5acd27cb1857 201
Thomas_H 0:fe156910301e 202 if(m_oNunchuck.buttonc()) {
Thomas_H 0:fe156910301e 203 m_oPc.printf("switch back to auto mode\n");
Thomas_H 0:fe156910301e 204 nControl = 0; // fallback to manual controll
Thomas_H 0:fe156910301e 205 }
Thomas_H 0:fe156910301e 206 }
Thomas_H 0:fe156910301e 207
Thomas_H 0:fe156910301e 208 oMotorRight.putc(256);
Thomas_H 1:5acd27cb1857 209 oMotorRight.putc(_Controlloop[RIGHT].u & 0xFF);
Thomas_H 1:5acd27cb1857 210 oMotorRight.putc((_Controlloop[RIGHT].u >> 8) & 0xFF);
Thomas_H 1:5acd27cb1857 211 oMotorRight.putc(_Controlloop[RIGHT].u & 0xFF);
Thomas_H 1:5acd27cb1857 212 oMotorRight.putc((_Controlloop[RIGHT].u >> 8) & 0xFF);
Thomas_H 0:fe156910301e 213 oMotorRight.putc(85);
Thomas_H 0:fe156910301e 214 oMotorRight.putc(82);
Thomas_H 0:fe156910301e 215 oMotorRight.putc(82);
Thomas_H 1:5acd27cb1857 216
Thomas_H 1:5acd27cb1857 217 wait_ms(1);
Thomas_H 1:5acd27cb1857 218
Thomas_H 0:fe156910301e 219 oMotorLeft.putc(256);
Thomas_H 1:5acd27cb1857 220 oMotorLeft.putc(_Controlloop[LEFT].u & 0xFF);
Thomas_H 1:5acd27cb1857 221 oMotorLeft.putc((_Controlloop[LEFT].u >> 8) & 0xFF);
Thomas_H 1:5acd27cb1857 222 oMotorLeft.putc(_Controlloop[LEFT].u & 0xFF);
Thomas_H 1:5acd27cb1857 223 oMotorLeft.putc((_Controlloop[LEFT].u >> 8) & 0xFF);
Thomas_H 0:fe156910301e 224 oMotorLeft.putc(85);
Thomas_H 0:fe156910301e 225 oMotorLeft.putc(82);
Thomas_H 0:fe156910301e 226 oMotorLeft.putc(82);
Thomas_H 1:5acd27cb1857 227
Thomas_H 1:5acd27cb1857 228
Thomas_H 0:fe156910301e 229 } while(!m_oPc.readable());
Thomas_H 0:fe156910301e 230 c=m_oPc.getc();
Thomas_H 0:fe156910301e 231 }
Thomas_H 0:fe156910301e 232 }
Thomas_H 0:fe156910301e 233