Thomas Hartmann / HoverboardTest

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   WiiNunchuck mbed-dev

main.cpp@1:5acd27cb1857, 2016-10-23 (annotated)

Committer:
Thomas_H
Date:
Sun Oct 23 19:35:14 2016 +0000
Revision:
1:5acd27cb1857
Parent:
0:fe156910301e 
Initial release

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