Zoltan Hudak / Mbed OS ScilabArduino

Data acquisition and device control with Scilab.

Dependencies:   Servo MPU6050

Desciription

Scilab is a freeware alternative to MATLAB. For low-cost data acquisition and device control a nice Arduino toolbox is available.

https://os.mbed.com/media/uploads/hudakz/scilab.png

This site presents a Mbed port which allows to use Mbed boards (equipped with Arduino header) rather than Arduino to import real time data into Scilab and to control real equipment witch Scilab.

https://os.mbed.com/media/uploads/hudakz/arduino-temp-read_imagelarge_1.jpeg

Installation
  • Install Scilab to your PC, if not done yet.
  • Launch Scilab and install the Arduino toolbox by executing the following command from the Scilab console: 

--> atomsInstall("arduino")
Controlling Mbed's digital output from Scilab
  • In Xcos open examples/Arduino1.zcos

/media/uploads/hudakz/scilab_arduino1.png

  • Double click on the Board setup block and replace the serial port number with mbed's actual virtual serial port number.
  • Double click on the Digital WRITE block and set Digital Pin to 13 (D13 is connected to LED1).
  • Start simulation and LED1 on the Mbed board should start blinking.
Reading and displaying Mbed's analog input
  • In Xcos open examples/Arduino2.zcos

/media/uploads/hudakz/scilab_arduino2.png

  • Double click on the Board setup block and replace the serial port number with mbed's actual virtual serial port number.
  • Double click on the Analog READ block and set Analog Pin to 2.
  • Start simulation and a graph should appear showing the analog signal measured on Mbed's pin A2.

NOTE: Currently, there is bug in the toolbox ARDUINO_ANALOG_READ_sim function (I have reported to Scilab) so the analog readings are not correct.

/media/uploads/hudakz/scilab_graph.png

PID controller
  • In Xcos open examples/Arduino9.zcos

/media/uploads/hudakz/scilab_arduino9.png

ScilabSerial/ScilabSerial.cpp@0:295b7e1c12f3, 2021-01-18 (annotated)

Committer:
hudakz
Date:
Mon Jan 18 19:51:22 2021 +0000
Revision:
0:295b7e1c12f3
Data acquisition and device control with Scilab.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hudakz 0:295b7e1c12f3 1 #include "ScilabSerial.h"
hudakz 0:295b7e1c12f3 2
hudakz 0:295b7e1c12f3 3 extern UnbufferedSerial dbgPort;
hudakz 0:295b7e1c12f3 4
hudakz 0:295b7e1c12f3 5 /**
hudakz 0:295b7e1c12f3 6 * @brief
hudakz 0:295b7e1c12f3 7 * @note
hudakz 0:295b7e1c12f3 8 * @param
hudakz 0:295b7e1c12f3 9 * @retval
hudakz 0:295b7e1c12f3 10 */
hudakz 0:295b7e1c12f3 11 ScilabSerial::ScilabSerial(PinName tx, PinName rx, int baud /*= 115200*/ ) :
hudakz 0:295b7e1c12f3 12 SerialBase(tx, rx, baud),
hudakz 0:295b7e1c12f3 13 _rxComplete(false),
hudakz 0:295b7e1c12f3 14 _rxBufLen(0),
hudakz 0:295b7e1c12f3 15 _rxDataLen(0),
hudakz 0:295b7e1c12f3 16 _deltaT(0),
hudakz 0:295b7e1c12f3 17 _mpu6050Available(false)
hudakz 0:295b7e1c12f3 18 {
hudakz 0:295b7e1c12f3 19 attach(callback(this, &ScilabSerial::onRx));
hudakz 0:295b7e1c12f3 20
hudakz 0:295b7e1c12f3 21 // Analog inputs
hudakz 0:295b7e1c12f3 22 for (size_t i = 0; i < sizeof(_analogPinName) / sizeof(*_analogPinName); i++) {
hudakz 0:295b7e1c12f3 23 // indexes -> pin names
hudakz 0:295b7e1c12f3 24 if (_analogPinName[i] != NC)
hudakz 0:295b7e1c12f3 25 _analogIns[i] = new AnalogIn(_analogPinName[i]);
hudakz 0:295b7e1c12f3 26 else
hudakz 0:295b7e1c12f3 27 _analogIns[i] = NULL;
hudakz 0:295b7e1c12f3 28 }
hudakz 0:295b7e1c12f3 29
hudakz 0:295b7e1c12f3 30 // PWM outputs
hudakz 0:295b7e1c12f3 31 for (size_t i = 0; i < 4; i++) {
hudakz 0:295b7e1c12f3 32 // indexes 0 to 3 -> pin names D4 to D7
hudakz 0:295b7e1c12f3 33 if (_digitalPinName[i + 4] != NC)
hudakz 0:295b7e1c12f3 34 _pwmOuts[i] = new PwmOut(_digitalPinName[i + 4]);
hudakz 0:295b7e1c12f3 35 else
hudakz 0:295b7e1c12f3 36 _pwmOuts[i] = NULL;
hudakz 0:295b7e1c12f3 37 }
hudakz 0:295b7e1c12f3 38
hudakz 0:295b7e1c12f3 39 // Digital read write
hudakz 0:295b7e1c12f3 40 for (size_t i = 0; i < sizeof(_digitalInOuts) / sizeof(*_digitalInOuts); i++)
hudakz 0:295b7e1c12f3 41 _digitalInOuts[i] = NULL;
hudakz 0:295b7e1c12f3 42
hudakz 0:295b7e1c12f3 43 // Servo
hudakz 0:295b7e1c12f3 44 for (size_t i = 0; i < sizeof(_servos) / sizeof(*_servos); i++)
hudakz 0:295b7e1c12f3 45 _servos[i] = NULL;
hudakz 0:295b7e1c12f3 46
hudakz 0:295b7e1c12f3 47 // MPU6050
hudakz 0:295b7e1c12f3 48 _mpu6050 = NULL;
hudakz 0:295b7e1c12f3 49
hudakz 0:295b7e1c12f3 50 // Interrup-in counters
hudakz 0:295b7e1c12f3 51 for (size_t i = 0; i < sizeof(_intrInCounters) / sizeof(*_intrInCounters); i++)
hudakz 0:295b7e1c12f3 52 _intrInCounters[i] = NULL;
hudakz 0:295b7e1c12f3 53
hudakz 0:295b7e1c12f3 54 // DC motors
hudakz 0:295b7e1c12f3 55 for (size_t i = 0; i < sizeof(_dcMotors) / sizeof(*_dcMotors); i++)
hudakz 0:295b7e1c12f3 56 _dcMotors[i] = NULL;
hudakz 0:295b7e1c12f3 57 }
hudakz 0:295b7e1c12f3 58
hudakz 0:295b7e1c12f3 59 /**
hudakz 0:295b7e1c12f3 60 * @brief
hudakz 0:295b7e1c12f3 61 * @note
hudakz 0:295b7e1c12f3 62 * @param
hudakz 0:295b7e1c12f3 63 * @retval
hudakz 0:295b7e1c12f3 64 */
hudakz 0:295b7e1c12f3 65 void ScilabSerial::onRx()
hudakz 0:295b7e1c12f3 66 {
hudakz 0:295b7e1c12f3 67 // If the '\n' character was used as 'packet-end' delimiter then we could have:
hudakz 0:295b7e1c12f3 68 //
hudakz 0:295b7e1c12f3 69 // while (readable()) {
hudakz 0:295b7e1c12f3 70 // volatile char c = _base_getc();
hudakz 0:295b7e1c12f3 71 // if (c != '\n') {
hudakz 0:295b7e1c12f3 72 // if (rxBufLen >= RX_MAX)
hudakz 0:295b7e1c12f3 73 // rxBufLen = 0; // rxBuf overflow
hudakz 0:295b7e1c12f3 74 // rxBuf[rxBufLen++] = c;
hudakz 0:295b7e1c12f3 75 // }
hudakz 0:295b7e1c12f3 76 // else {
hudakz 0:295b7e1c12f3 77 // // packet complete
hudakz 0:295b7e1c12f3 78 // memset(rxData, 0, RX_MAX); // clear rxData
hudakz 0:295b7e1c12f3 79 // memcpy(rxData, rxBuf, rxBufLen); // copy rxBuf to rxData
hudakz 0:295b7e1c12f3 80 // rxDataLen = rxBufLen; // set rxData length
hudakz 0:295b7e1c12f3 81 // memset(rxBuf, 0, RX_MAX); // clear rxBuf
hudakz 0:295b7e1c12f3 82 // rxBufLen = 0;
hudakz 0:295b7e1c12f3 83 // }
hudakz 0:295b7e1c12f3 84 // }
hudakz 0:295b7e1c12f3 85 // Unfortunately there is neither packet length information in the packet
hudakz 0:295b7e1c12f3 86 // nor 'packet-end' delimiter is used.
hudakz 0:295b7e1c12f3 87 // That's why we have to figure out the packet length on the fly based on it's content:
hudakz 0:295b7e1c12f3 88 volatile char c = _base_getc(); // Read a byte from UART
hudakz 0:295b7e1c12f3 89
hudakz 0:295b7e1c12f3 90 _rxBuf[_rxBufLen++] = c; // Store the byte in the receive buffer
hudakz 0:295b7e1c12f3 91
hudakz 0:295b7e1c12f3 92 switch (_rxBuf[0]) {
hudakz 0:295b7e1c12f3 93 case 'A': // Analog read
hudakz 0:295b7e1c12f3 94 case 'G': // MPU6050
hudakz 0:295b7e1c12f3 95 case 'R': // Revision request
hudakz 0:295b7e1c12f3 96 if (_rxBufLen == 2)
hudakz 0:295b7e1c12f3 97 _rxComplete = true;
hudakz 0:295b7e1c12f3 98 break;
hudakz 0:295b7e1c12f3 99
hudakz 0:295b7e1c12f3 100 case 'W': // PWM (analog) write
hudakz 0:295b7e1c12f3 101 case 'I': // Interrupt-in counter
hudakz 0:295b7e1c12f3 102 if (_rxBufLen == 3)
hudakz 0:295b7e1c12f3 103 _rxComplete = true;
hudakz 0:295b7e1c12f3 104 break;
hudakz 0:295b7e1c12f3 105
hudakz 0:295b7e1c12f3 106 case 'D': // Digital read/write
hudakz 0:295b7e1c12f3 107 if (_rxBufLen == 3)
hudakz 0:295b7e1c12f3 108 if (_rxBuf[1] == 'r')
hudakz 0:295b7e1c12f3 109 _rxComplete = true;
hudakz 0:295b7e1c12f3 110 break;
hudakz 0:295b7e1c12f3 111
hudakz 0:295b7e1c12f3 112 case 'S': // Servo motor
hudakz 0:295b7e1c12f3 113 if (_rxBufLen == 3)
hudakz 0:295b7e1c12f3 114 if (_rxBuf[1] != 'w')
hudakz 0:295b7e1c12f3 115 _rxComplete = true;
hudakz 0:295b7e1c12f3 116 break;
hudakz 0:295b7e1c12f3 117
hudakz 0:295b7e1c12f3 118 case 'E': // Encoder
hudakz 0:295b7e1c12f3 119 if (_rxBufLen == 3)
hudakz 0:295b7e1c12f3 120 if (_rxBuf[1] != 'a')
hudakz 0:295b7e1c12f3 121 _rxComplete = true;
hudakz 0:295b7e1c12f3 122 break;
hudakz 0:295b7e1c12f3 123
hudakz 0:295b7e1c12f3 124 case 'C':
hudakz 0:295b7e1c12f3 125 if (_rxBufLen == 5)
hudakz 0:295b7e1c12f3 126 _rxComplete = true;
hudakz 0:295b7e1c12f3 127
hudakz 0:295b7e1c12f3 128 case 'M': // DC motor
hudakz 0:295b7e1c12f3 129 if (_rxBufLen == 3)
hudakz 0:295b7e1c12f3 130 if (_rxBuf[1] == 'r')
hudakz 0:295b7e1c12f3 131 _rxComplete = true;
hudakz 0:295b7e1c12f3 132 break;
hudakz 0:295b7e1c12f3 133 }
hudakz 0:295b7e1c12f3 134
hudakz 0:295b7e1c12f3 135 if ((_rxBuf[0] != 'C') && (_rxBufLen == 4))
hudakz 0:295b7e1c12f3 136 _rxComplete = true;
hudakz 0:295b7e1c12f3 137
hudakz 0:295b7e1c12f3 138 if (_rxComplete) {
hudakz 0:295b7e1c12f3 139 _rxComplete = false;
hudakz 0:295b7e1c12f3 140 memset(_rxData, 0, RX_MAX); // reset rxData
hudakz 0:295b7e1c12f3 141 memcpy(_rxData, _rxBuf, _rxBufLen); // copy rxBuf to rxData
hudakz 0:295b7e1c12f3 142 _rxDataLen = _rxBufLen; // set rxData length
hudakz 0:295b7e1c12f3 143 memset(_rxBuf, 0, RX_MAX); // reset rxBuf
hudakz 0:295b7e1c12f3 144 _rxBufLen = 0;
hudakz 0:295b7e1c12f3 145 }
hudakz 0:295b7e1c12f3 146
hudakz 0:295b7e1c12f3 147 // dbgPort.write((void*)(& _rxData), _rxDataLen);
hudakz 0:295b7e1c12f3 148 // dbgPort.write("\r\n", 2);
hudakz 0:295b7e1c12f3 149
hudakz 0:295b7e1c12f3 150 }
hudakz 0:295b7e1c12f3 151
hudakz 0:295b7e1c12f3 152 /**
hudakz 0:295b7e1c12f3 153 * @brief
hudakz 0:295b7e1c12f3 154 * @note
hudakz 0:295b7e1c12f3 155 * @param
hudakz 0:295b7e1c12f3 156 * @retval
hudakz 0:295b7e1c12f3 157 */
hudakz 0:295b7e1c12f3 158 ssize_t ScilabSerial::write(const void* buffer, size_t length)
hudakz 0:295b7e1c12f3 159 {
hudakz 0:295b7e1c12f3 160 const char* ptr = (const char*)buffer;
hudakz 0:295b7e1c12f3 161 const char* end = ptr + length;
hudakz 0:295b7e1c12f3 162
hudakz 0:295b7e1c12f3 163 lock();
hudakz 0:295b7e1c12f3 164 while (ptr != end) {
hudakz 0:295b7e1c12f3 165 _base_putc(*ptr++);
hudakz 0:295b7e1c12f3 166 }
hudakz 0:295b7e1c12f3 167
hudakz 0:295b7e1c12f3 168 unlock();
hudakz 0:295b7e1c12f3 169
hudakz 0:295b7e1c12f3 170 return ptr - (const char*)buffer;
hudakz 0:295b7e1c12f3 171 }
hudakz 0:295b7e1c12f3 172
hudakz 0:295b7e1c12f3 173 /**
hudakz 0:295b7e1c12f3 174 * @brief
hudakz 0:295b7e1c12f3 175 * @note
hudakz 0:295b7e1c12f3 176 * @param
hudakz 0:295b7e1c12f3 177 * @retval
hudakz 0:295b7e1c12f3 178 */
hudakz 0:295b7e1c12f3 179 void ScilabSerial::run()
hudakz 0:295b7e1c12f3 180 {
hudakz 0:295b7e1c12f3 181 while (1) {
hudakz 0:295b7e1c12f3 182 if (_mpu6050Available)
hudakz 0:295b7e1c12f3 183 mpu6050UpdateData();
hudakz 0:295b7e1c12f3 184
hudakz 0:295b7e1c12f3 185 if (_rxDataLen > 0) {
hudakz 0:295b7e1c12f3 186 _rxDataLen = 0;
hudakz 0:295b7e1c12f3 187 switch (_rxData[0]) {
hudakz 0:295b7e1c12f3 188 case 'A': // Analog read
hudakz 0:295b7e1c12f3 189 analogRead();
hudakz 0:295b7e1c12f3 190 break;
hudakz 0:295b7e1c12f3 191
hudakz 0:295b7e1c12f3 192 case 'W': // PWM (analog) write
hudakz 0:295b7e1c12f3 193 pwmWrite();
hudakz 0:295b7e1c12f3 194 break;
hudakz 0:295b7e1c12f3 195
hudakz 0:295b7e1c12f3 196 case 'D': // Digital
hudakz 0:295b7e1c12f3 197 digitalReadWrite();
hudakz 0:295b7e1c12f3 198 break;
hudakz 0:295b7e1c12f3 199
hudakz 0:295b7e1c12f3 200 case 'S': // Servo motor
hudakz 0:295b7e1c12f3 201 servo();
hudakz 0:295b7e1c12f3 202 break;
hudakz 0:295b7e1c12f3 203
hudakz 0:295b7e1c12f3 204 case 'G': // MPU6050
hudakz 0:295b7e1c12f3 205 mpu6050();
hudakz 0:295b7e1c12f3 206 break;
hudakz 0:295b7e1c12f3 207
hudakz 0:295b7e1c12f3 208 case 'I': // Interrupt-in counter
hudakz 0:295b7e1c12f3 209 intrInCounter();
hudakz 0:295b7e1c12f3 210 break;
hudakz 0:295b7e1c12f3 211
hudakz 0:295b7e1c12f3 212 case 'E': // Encoder
hudakz 0:295b7e1c12f3 213 encoder();
hudakz 0:295b7e1c12f3 214 break;
hudakz 0:295b7e1c12f3 215
hudakz 0:295b7e1c12f3 216 case 'C': // DC motor init
hudakz 0:295b7e1c12f3 217 dcMotorInit();
hudakz 0:295b7e1c12f3 218 break;
hudakz 0:295b7e1c12f3 219
hudakz 0:295b7e1c12f3 220 case 'M': // DC motor
hudakz 0:295b7e1c12f3 221 dcMotor();
hudakz 0:295b7e1c12f3 222 break;
hudakz 0:295b7e1c12f3 223
hudakz 0:295b7e1c12f3 224 case 'R': // Revision
hudakz 0:295b7e1c12f3 225 revision();
hudakz 0:295b7e1c12f3 226 break;
hudakz 0:295b7e1c12f3 227 }
hudakz 0:295b7e1c12f3 228 }
hudakz 0:295b7e1c12f3 229 }
hudakz 0:295b7e1c12f3 230 }
hudakz 0:295b7e1c12f3 231
hudakz 0:295b7e1c12f3 232 /**
hudakz 0:295b7e1c12f3 233 * @brief
hudakz 0:295b7e1c12f3 234 * @note
hudakz 0:295b7e1c12f3 235 * @param
hudakz 0:295b7e1c12f3 236 * @retval
hudakz 0:295b7e1c12f3 237 */
hudakz 0:295b7e1c12f3 238 void ScilabSerial::analogRead()
hudakz 0:295b7e1c12f3 239 {
hudakz 0:295b7e1c12f3 240 int pin = _rxData[1] - '0'; // analog pin number 0 .. 5
hudakz 0:295b7e1c12f3 241 uint8_t i = pin; // index of corresponding analogIn 0 .. 5
hudakz 0:295b7e1c12f3 242
hudakz 0:295b7e1c12f3 243 if ((pin >= 0 && pin <= 5) && (_analogIns[i] != NULL)) {
hudakz 0:295b7e1c12f3 244 uint16_t word = _analogIns[i]->read_u16(); // get decimal value 0 .. 65535
hudakz 0:295b7e1c12f3 245 float val = word / 64.0f; // map 16bit range to 10bit range
hudakz 0:295b7e1c12f3 246
hudakz 0:295b7e1c12f3 247 word = round(val);
hudakz 0:295b7e1c12f3 248 write((uint8_t*) &word, sizeof(uint16_t)); // send it to Scilab
hudakz 0:295b7e1c12f3 249 }
hudakz 0:295b7e1c12f3 250 }
hudakz 0:295b7e1c12f3 251
hudakz 0:295b7e1c12f3 252 /**
hudakz 0:295b7e1c12f3 253 * @brief
hudakz 0:295b7e1c12f3 254 * @note
hudakz 0:295b7e1c12f3 255 * @param
hudakz 0:295b7e1c12f3 256 * @retval
hudakz 0:295b7e1c12f3 257 */
hudakz 0:295b7e1c12f3 258 void ScilabSerial::pwmWrite()
hudakz 0:295b7e1c12f3 259 {
hudakz 0:295b7e1c12f3 260 int pin = _rxData[1] - '0'; // digital pin number 4 .. 7
hudakz 0:295b7e1c12f3 261 uint8_t i = pin - 4; // index of corresponding analogOut (PWMOut) 0 .. 3
hudakz 0:295b7e1c12f3 262
hudakz 0:295b7e1c12f3 263 if ((pin >= 4 && pin <= 7) && (_pwmOuts[i] != NULL)) {
hudakz 0:295b7e1c12f3 264 uint8_t val = _rxData[2]; // the duty cycle: between 0 (always off) and 255 (always on)
hudakz 0:295b7e1c12f3 265 float dutyCycle = (float)val / 255.0f; // 0.0f (representing on 0%) and 1.0f (representing on 100%)
hudakz 0:295b7e1c12f3 266
hudakz 0:295b7e1c12f3 267 _pwmOuts[i]->write(dutyCycle);
hudakz 0:295b7e1c12f3 268 }
hudakz 0:295b7e1c12f3 269 }
hudakz 0:295b7e1c12f3 270
hudakz 0:295b7e1c12f3 271 /**
hudakz 0:295b7e1c12f3 272 * @brief
hudakz 0:295b7e1c12f3 273 * @note
hudakz 0:295b7e1c12f3 274 * @param
hudakz 0:295b7e1c12f3 275 * @retval
hudakz 0:295b7e1c12f3 276 */
hudakz 0:295b7e1c12f3 277 void ScilabSerial::digitalReadWrite()
hudakz 0:295b7e1c12f3 278 {
hudakz 0:295b7e1c12f3 279 int pin = _rxData[2] - '0'; // digital pin number 8 .. 13
hudakz 0:295b7e1c12f3 280 uint8_t i = pin - 8; // index of corresponding digitalInOut 0 .. 7
hudakz 0:295b7e1c12f3 281
hudakz 0:295b7e1c12f3 282 if ((pin >= 8 && pin <= 13) && (_digitalPinName[pin] != NC)) {
hudakz 0:295b7e1c12f3 283 char cmd = _rxData[1]; // command
hudakz 0:295b7e1c12f3 284 PinDirection pinDirection; // pin direction (PIN_INPUT, PIN_OUTPUT)
hudakz 0:295b7e1c12f3 285 char c;
hudakz 0:295b7e1c12f3 286 uint8_t val;
hudakz 0:295b7e1c12f3 287
hudakz 0:295b7e1c12f3 288 switch (cmd) {
hudakz 0:295b7e1c12f3 289 case 'a': // 'a'-> activate
hudakz 0:295b7e1c12f3 290 pinDirection = (PinDirection) (_rxData[3] - '0');
hudakz 0:295b7e1c12f3 291 if (_digitalInOuts[i] != NULL)
hudakz 0:295b7e1c12f3 292 delete _digitalInOuts[i];
hudakz 0:295b7e1c12f3 293 _digitalInOuts[i] = new DigitalInOut(_digitalPinName[pin], pinDirection, PullUp, 0);
hudakz 0:295b7e1c12f3 294 break;
hudakz 0:295b7e1c12f3 295
hudakz 0:295b7e1c12f3 296 case 'r': // 'r'-> read
hudakz 0:295b7e1c12f3 297 c = _digitalInOuts[i]->read() + '0';
hudakz 0:295b7e1c12f3 298 write((uint8_t*) &c, 1);
hudakz 0:295b7e1c12f3 299 break;
hudakz 0:295b7e1c12f3 300
hudakz 0:295b7e1c12f3 301 case 'w': // 'w'-> write
hudakz 0:295b7e1c12f3 302 val = _rxData[3] - '0';
hudakz 0:295b7e1c12f3 303
hudakz 0:295b7e1c12f3 304 if (val == 0 || val == 1)
hudakz 0:295b7e1c12f3 305 _digitalInOuts[i]->write(val);
hudakz 0:295b7e1c12f3 306 break;
hudakz 0:295b7e1c12f3 307 }
hudakz 0:295b7e1c12f3 308 }
hudakz 0:295b7e1c12f3 309 else
hudakz 0:295b7e1c12f3 310 _digitalInOuts[i] = NULL;
hudakz 0:295b7e1c12f3 311 }
hudakz 0:295b7e1c12f3 312
hudakz 0:295b7e1c12f3 313 /**
hudakz 0:295b7e1c12f3 314 * @brief
hudakz 0:295b7e1c12f3 315 * @note
hudakz 0:295b7e1c12f3 316 * @param
hudakz 0:295b7e1c12f3 317 * @retval
hudakz 0:295b7e1c12f3 318 */
hudakz 0:295b7e1c12f3 319 void ScilabSerial::servo()
hudakz 0:295b7e1c12f3 320 {
hudakz 0:295b7e1c12f3 321 int pin = _rxData[2] - '0' + 1; // digital pin number 2 .. 3
hudakz 0:295b7e1c12f3 322 uint8_t i = pin - 2; // index of corresponding servo 0 .. 1
hudakz 0:295b7e1c12f3 323 uint8_t val;
hudakz 0:295b7e1c12f3 324
hudakz 0:295b7e1c12f3 325 if ((pin == 2 || pin == 3) && (_digitalPinName[pin] != NC)) {
hudakz 0:295b7e1c12f3 326 char cmd = _rxData[1];
hudakz 0:295b7e1c12f3 327 switch (cmd) {
hudakz 0:295b7e1c12f3 328 case 'a': // 'a' -> activate servo
hudakz 0:295b7e1c12f3 329 if (_servos[i] != NULL)
hudakz 0:295b7e1c12f3 330 delete _servos[i];
hudakz 0:295b7e1c12f3 331 _servos[i] = new Servo(_digitalPinName[pin]);
hudakz 0:295b7e1c12f3 332 break;
hudakz 0:295b7e1c12f3 333
hudakz 0:295b7e1c12f3 334 case 'd': // 'd'-> delete servo
hudakz 0:295b7e1c12f3 335 delete _servos[i];
hudakz 0:295b7e1c12f3 336 _servos[i] = NULL;
hudakz 0:295b7e1c12f3 337 break;
hudakz 0:295b7e1c12f3 338
hudakz 0:295b7e1c12f3 339 case 'w': // 'w'-> write to servo position in degree (0 .. 180)
hudakz 0:295b7e1c12f3 340 val = _rxData[3];
hudakz 0:295b7e1c12f3 341 if (val >= 0 && val <= 180)
hudakz 0:295b7e1c12f3 342 _servos[i]->position(val);
hudakz 0:295b7e1c12f3 343 break;
hudakz 0:295b7e1c12f3 344 }
hudakz 0:295b7e1c12f3 345 }
hudakz 0:295b7e1c12f3 346 else
hudakz 0:295b7e1c12f3 347 _servos[i] = NULL;
hudakz 0:295b7e1c12f3 348 }
hudakz 0:295b7e1c12f3 349
hudakz 0:295b7e1c12f3 350 /**
hudakz 0:295b7e1c12f3 351 * @brief
hudakz 0:295b7e1c12f3 352 * @note
hudakz 0:295b7e1c12f3 353 * @param
hudakz 0:295b7e1c12f3 354 * @retval
hudakz 0:295b7e1c12f3 355 */
hudakz 0:295b7e1c12f3 356 void ScilabSerial::mpu6050()
hudakz 0:295b7e1c12f3 357 {
hudakz 0:295b7e1c12f3 358 uint8_t cmd = _rxData[1];
hudakz 0:295b7e1c12f3 359 uint16_t data[9];
hudakz 0:295b7e1c12f3 360
hudakz 0:295b7e1c12f3 361 if (I2C_SCL != NC && I2C_SDA != NC) {
hudakz 0:295b7e1c12f3 362 switch (cmd) {
hudakz 0:295b7e1c12f3 363 case 'a': // 'a'-> acivate
hudakz 0:295b7e1c12f3 364 if (_mpu6050 != NULL)
hudakz 0:295b7e1c12f3 365 delete _mpu6050;
hudakz 0:295b7e1c12f3 366 _mpu6050 = new MPU6050();
hudakz 0:295b7e1c12f3 367 _mpu6050Available = _mpu6050->init();
hudakz 0:295b7e1c12f3 368 _mpu6050Timer.start();
hudakz 0:295b7e1c12f3 369 break;
hudakz 0:295b7e1c12f3 370
hudakz 0:295b7e1c12f3 371 case 't': // 't' -> test
hudakz 0:295b7e1c12f3 372 if (_mpu6050->selfTestOK()) {
hudakz 0:295b7e1c12f3 373 _mpu6050->reset(); // Reset registers to default in preparation for device calibration
hudakz 0:295b7e1c12f3 374 _mpu6050->calibrate(); // Calibrate gyro and accelerometers, load biases in bias registers
hudakz 0:295b7e1c12f3 375 _mpu6050Available = _mpu6050->init();
hudakz 0:295b7e1c12f3 376 _mpu6050Timer.reset();
hudakz 0:295b7e1c12f3 377 _mpu6050Timer.start();
hudakz 0:295b7e1c12f3 378 write("OK", 2);
hudakz 0:295b7e1c12f3 379 }
hudakz 0:295b7e1c12f3 380 else {
hudakz 0:295b7e1c12f3 381 _mpu6050Available = false;
hudakz 0:295b7e1c12f3 382 write("KO", 2);
hudakz 0:295b7e1c12f3 383 }
hudakz 0:295b7e1c12f3 384 break;
hudakz 0:295b7e1c12f3 385
hudakz 0:295b7e1c12f3 386 case 'r': // 'r'-> read
hudakz 0:295b7e1c12f3 387 if (_mpu6050Available) {
hudakz 0:295b7e1c12f3 388 data[0] = _mpu6050->yaw();
hudakz 0:295b7e1c12f3 389 data[1] = _mpu6050->pitch();
hudakz 0:295b7e1c12f3 390 data[2] = _mpu6050->roll();
hudakz 0:295b7e1c12f3 391 data[3] = _mpu6050->accelX;
hudakz 0:295b7e1c12f3 392 data[4] = _mpu6050->accelY;
hudakz 0:295b7e1c12f3 393 data[5] = _mpu6050->accelZ;
hudakz 0:295b7e1c12f3 394 data[6] = _mpu6050->gyroX;
hudakz 0:295b7e1c12f3 395 data[7] = _mpu6050->gyroY;
hudakz 0:295b7e1c12f3 396 data[8] = _mpu6050->gyroZ;
hudakz 0:295b7e1c12f3 397 write(data, sizeof(data)); // send data to scilab
hudakz 0:295b7e1c12f3 398 }
hudakz 0:295b7e1c12f3 399 break;
hudakz 0:295b7e1c12f3 400 }
hudakz 0:295b7e1c12f3 401 }
hudakz 0:295b7e1c12f3 402 else
hudakz 0:295b7e1c12f3 403 _mpu6050 = NULL;
hudakz 0:295b7e1c12f3 404 }
hudakz 0:295b7e1c12f3 405
hudakz 0:295b7e1c12f3 406 /**
hudakz 0:295b7e1c12f3 407 * @brief
hudakz 0:295b7e1c12f3 408 * @note
hudakz 0:295b7e1c12f3 409 * @param
hudakz 0:295b7e1c12f3 410 * @retval
hudakz 0:295b7e1c12f3 411 */
hudakz 0:295b7e1c12f3 412 void ScilabSerial::mpu6050UpdateData()
hudakz 0:295b7e1c12f3 413 {
hudakz 0:295b7e1c12f3 414 // check if data is available after interrupt
hudakz 0:295b7e1c12f3 415
hudakz 0:295b7e1c12f3 416 if (_mpu6050->dataReady()) {
hudakz 0:295b7e1c12f3 417 _mpu6050->accel();
hudakz 0:295b7e1c12f3 418 _mpu6050->gyro();
hudakz 0:295b7e1c12f3 419
hudakz 0:295b7e1c12f3 420 uint32_t _now = _mpu6050Timer.read_us();
hudakz 0:295b7e1c12f3 421 _mpu6050Timer.reset();
hudakz 0:295b7e1c12f3 422
hudakz 0:295b7e1c12f3 423 float _deltaT = float(_now / 1e-6); // set integration time in seconds
hudakz 0:295b7e1c12f3 424
hudakz 0:295b7e1c12f3 425 _mpu6050->madgwickFilter(_deltaT); // apply filter to estimate yaw, pitch and roll
hudakz 0:295b7e1c12f3 426 }
hudakz 0:295b7e1c12f3 427 }
hudakz 0:295b7e1c12f3 428
hudakz 0:295b7e1c12f3 429 /**
hudakz 0:295b7e1c12f3 430 * @brief
hudakz 0:295b7e1c12f3 431 * @note
hudakz 0:295b7e1c12f3 432 * @param
hudakz 0:295b7e1c12f3 433 * @retval
hudakz 0:295b7e1c12f3 434 */
hudakz 0:295b7e1c12f3 435 void ScilabSerial::intrInCounter()
hudakz 0:295b7e1c12f3 436 {
hudakz 0:295b7e1c12f3 437 int pin = _rxData[2] - '0'; // digital pin number 8 .. 13
hudakz 0:295b7e1c12f3 438 uint8_t i = pin - 8; // index of corresonding IntCounter 0 .. 7
hudakz 0:295b7e1c12f3 439
hudakz 0:295b7e1c12f3 440 if ((pin >= 8 && pin <= 13) && (_digitalPinName[pin] != NC)) {
hudakz 0:295b7e1c12f3 441 uint8_t val;
hudakz 0:295b7e1c12f3 442 char cmd = _rxData[1];
hudakz 0:295b7e1c12f3 443 switch (cmd) {
hudakz 0:295b7e1c12f3 444 case 'a': // 'a' -> activate a counter
hudakz 0:295b7e1c12f3 445 if (_intrInCounters[i] != NULL)
hudakz 0:295b7e1c12f3 446 delete _intrInCounters[i];
hudakz 0:295b7e1c12f3 447 _intrInCounters[i] = new IntrInCounter(_digitalPinName[pin]);
hudakz 0:295b7e1c12f3 448 break;
hudakz 0:295b7e1c12f3 449
hudakz 0:295b7e1c12f3 450 case 'p': // 'p' -> get position of a counter
hudakz 0:295b7e1c12f3 451 val = _intrInCounters[i]->read();
hudakz 0:295b7e1c12f3 452 write(&val, 1); // send value to scilab (0 .. 256)
hudakz 0:295b7e1c12f3 453 break;
hudakz 0:295b7e1c12f3 454
hudakz 0:295b7e1c12f3 455 case 'r': // 'r' -> release an interrupt counter
hudakz 0:295b7e1c12f3 456 delete _intrInCounters[i];
hudakz 0:295b7e1c12f3 457 break;
hudakz 0:295b7e1c12f3 458
hudakz 0:295b7e1c12f3 459 case 'z': // 'z' -> set to zero (reset) a counter
hudakz 0:295b7e1c12f3 460 _intrInCounters[i]->reset();
hudakz 0:295b7e1c12f3 461 break;
hudakz 0:295b7e1c12f3 462 }
hudakz 0:295b7e1c12f3 463 }
hudakz 0:295b7e1c12f3 464 else
hudakz 0:295b7e1c12f3 465 _intrInCounters[i] = NULL;
hudakz 0:295b7e1c12f3 466 }
hudakz 0:295b7e1c12f3 467
hudakz 0:295b7e1c12f3 468 /**
hudakz 0:295b7e1c12f3 469 * @brief
hudakz 0:295b7e1c12f3 470 * @note
hudakz 0:295b7e1c12f3 471 * @param
hudakz 0:295b7e1c12f3 472 * @retval
hudakz 0:295b7e1c12f3 473 */
hudakz 0:295b7e1c12f3 474 void ScilabSerial::encoder()
hudakz 0:295b7e1c12f3 475 {
hudakz 0:295b7e1c12f3 476 uint8_t i = _rxData[2] - '0'; // encoder index 0 .. 1
hudakz 0:295b7e1c12f3 477
hudakz 0:295b7e1c12f3 478 if ((i >= 0 && i <= 1) && (_digitalPinName[i])) {
hudakz 0:295b7e1c12f3 479 int pinA, pinB;
hudakz 0:295b7e1c12f3 480 int8_t pos;
hudakz 0:295b7e1c12f3 481 int mode;
hudakz 0:295b7e1c12f3 482 char cmd = _rxData[1]; // command
hudakz 0:295b7e1c12f3 483
hudakz 0:295b7e1c12f3 484 switch (cmd) {
hudakz 0:295b7e1c12f3 485 case 'a': // 'a' -> activate an encoder
hudakz 0:295b7e1c12f3 486 i == 0 ? pinA = 8 : pinA = 10;
hudakz 0:295b7e1c12f3 487 pinB = pinA + 1; // pinB -> 9 or 11
hudakz 0:295b7e1c12f3 488 mode = _rxData[3] - '0'; // '1' -> up chanA, '2' -> up/down chanA, '4' -> up/down chanA and chanB
hudakz 0:295b7e1c12f3 489 if (_encoders[i] != NULL)
hudakz 0:295b7e1c12f3 490 delete _encoders[i];
hudakz 0:295b7e1c12f3 491 _encoders[i] = new Encoder(_digitalPinName[pinA], _digitalPinName[pinB], mode);
hudakz 0:295b7e1c12f3 492 break;
hudakz 0:295b7e1c12f3 493
hudakz 0:295b7e1c12f3 494 case 'p': // 'p' -> get position of an encoder
hudakz 0:295b7e1c12f3 495 pos = _encoders[i]->position();
hudakz 0:295b7e1c12f3 496
hudakz 0:295b7e1c12f3 497 write(&pos, 1); // send value to scilab (0 .. 256)
hudakz 0:295b7e1c12f3 498 break;
hudakz 0:295b7e1c12f3 499
hudakz 0:295b7e1c12f3 500 case 'r': // 'r' -> release an encoder
hudakz 0:295b7e1c12f3 501 delete _encoders[i];
hudakz 0:295b7e1c12f3 502 break;
hudakz 0:295b7e1c12f3 503
hudakz 0:295b7e1c12f3 504 case 'z': // 'z' -> set to zero (reset) the position of an encoder
hudakz 0:295b7e1c12f3 505 _encoders[i]->reset();
hudakz 0:295b7e1c12f3 506 break;
hudakz 0:295b7e1c12f3 507 }
hudakz 0:295b7e1c12f3 508 }
hudakz 0:295b7e1c12f3 509 else
hudakz 0:295b7e1c12f3 510 _encoders[i] = NULL;
hudakz 0:295b7e1c12f3 511 }
hudakz 0:295b7e1c12f3 512
hudakz 0:295b7e1c12f3 513 /**
hudakz 0:295b7e1c12f3 514 * @brief
hudakz 0:295b7e1c12f3 515 * @note
hudakz 0:295b7e1c12f3 516 * @param
hudakz 0:295b7e1c12f3 517 * @retval
hudakz 0:295b7e1c12f3 518 */
hudakz 0:295b7e1c12f3 519 void ScilabSerial::dcMotorInit()
hudakz 0:295b7e1c12f3 520 {
hudakz 0:295b7e1c12f3 521 uint8_t pin1, pin2;
hudakz 0:295b7e1c12f3 522 uint8_t driverType;
hudakz 0:295b7e1c12f3 523 uint8_t i = _rxData[1] - '0'; // motor index
hudakz 0:295b7e1c12f3 524
hudakz 0:295b7e1c12f3 525 if ((i < 2) && (_digitalPinName[i] != NC)) {
hudakz 0:295b7e1c12f3 526 pin1 = _rxData[2]; // pin1 is 2 or 4
hudakz 0:295b7e1c12f3 527 if (pin1 == 2 || pin1 == 4) {
hudakz 0:295b7e1c12f3 528 pin2 = _rxData[3] - '0'; // pin2 is 3 or 5
hudakz 0:295b7e1c12f3 529 if (pin2 == 3 || pin2 == 5) {
hudakz 0:295b7e1c12f3 530 driverType = _rxData[4] - '0'; // driver: 0 -> L293, 1 -> L298
hudakz 0:295b7e1c12f3 531 if (driverType >= 0 && driverType <= 1) {
hudakz 0:295b7e1c12f3 532 if (_dcMotors[i] != NULL)
hudakz 0:295b7e1c12f3 533 delete _dcMotors[i];
hudakz 0:295b7e1c12f3 534 _dcMotors[i] = new DcMotor(_digitalPinName[pin1], _digitalPinName[pin2], driverType);
hudakz 0:295b7e1c12f3 535 write("OK", 2); // tell scilab that motor initialization is complete
hudakz 0:295b7e1c12f3 536 }
hudakz 0:295b7e1c12f3 537 }
hudakz 0:295b7e1c12f3 538 }
hudakz 0:295b7e1c12f3 539 }
hudakz 0:295b7e1c12f3 540 else
hudakz 0:295b7e1c12f3 541 _dcMotors[i] = NULL;
hudakz 0:295b7e1c12f3 542 }
hudakz 0:295b7e1c12f3 543
hudakz 0:295b7e1c12f3 544 /**
hudakz 0:295b7e1c12f3 545 * @brief
hudakz 0:295b7e1c12f3 546 * @note
hudakz 0:295b7e1c12f3 547 * @param
hudakz 0:295b7e1c12f3 548 * @retval
hudakz 0:295b7e1c12f3 549 */
hudakz 0:295b7e1c12f3 550 void ScilabSerial::dcMotor()
hudakz 0:295b7e1c12f3 551 {
hudakz 0:295b7e1c12f3 552 int pin1, pin2;
hudakz 0:295b7e1c12f3 553 int mode;
hudakz 0:295b7e1c12f3 554 char cmd;
hudakz 0:295b7e1c12f3 555 float speed;
hudakz 0:295b7e1c12f3 556 uint8_t i = _rxData[1] - '0'; // motor index
hudakz 0:295b7e1c12f3 557
hudakz 0:295b7e1c12f3 558 if ((i < 2) && (_dcMotors[i] != NULL)) {
hudakz 0:295b7e1c12f3 559 cmd = _rxData[2] - '0'; // direction of rotation or release
hudakz 0:295b7e1c12f3 560 switch (cmd) {
hudakz 0:295b7e1c12f3 561 case 0: // clockwise
hudakz 0:295b7e1c12f3 562 case 1: // counter clockwise
hudakz 0:295b7e1c12f3 563 speed = _rxData[3]; // speed
hudakz 0:295b7e1c12f3 564 _dcMotors[i]->run(cmd, speed);
hudakz 0:295b7e1c12f3 565 break;
hudakz 0:295b7e1c12f3 566
hudakz 0:295b7e1c12f3 567 case 'r': // release
hudakz 0:295b7e1c12f3 568 delete _dcMotors[i];
hudakz 0:295b7e1c12f3 569 break;
hudakz 0:295b7e1c12f3 570 }
hudakz 0:295b7e1c12f3 571 }
hudakz 0:295b7e1c12f3 572 }
hudakz 0:295b7e1c12f3 573
hudakz 0:295b7e1c12f3 574 /**
hudakz 0:295b7e1c12f3 575 * @brief
hudakz 0:295b7e1c12f3 576 * @note
hudakz 0:295b7e1c12f3 577 * @param
hudakz 0:295b7e1c12f3 578 * @retval
hudakz 0:295b7e1c12f3 579 */
hudakz 0:295b7e1c12f3 580 void ScilabSerial::revision()
hudakz 0:295b7e1c12f3 581 {
hudakz 0:295b7e1c12f3 582 write("v5", 2);
hudakz 0:295b7e1c12f3 583 }