auPilot / auSpeed

Test suite for auSpeed combo controller (ESC/power/servo) More info: http://aupilot.com.au

Dependencies:   mbed

main.cpp@0:d38b3edad9b3, 2015-03-17 (annotated)

Committer:
Kir
Date:
Tue Mar 17 08:46:26 2015 +0000
Revision:
0:d38b3edad9b3
release 1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kir 0:d38b3edad9b3 1 /*
Kir 0:d38b3edad9b3 2 Copyright (c) 2015 Aurobo Pty Ltd
Kir 0:d38b3edad9b3 3
Kir 0:d38b3edad9b3 4 === A test program for the auSpeed Integrated Combo board ===
Kir 0:d38b3edad9b3 5 Version 1.00
Kir 0:d38b3edad9b3 6
Kir 0:d38b3edad9b3 7 The auSpeed-1M combo controller is an integrated module that combines
Kir 0:d38b3edad9b3 8 three UAV sub-systems:
Kir 0:d38b3edad9b3 9 - high efficient protected power supply for electronic units and servos
Kir 0:d38b3edad9b3 10 with voltage and current sensors (BEC);
Kir 0:d38b3edad9b3 11 - brushless motor speed controller (ESC), up to 40A;
Kir 0:d38b3edad9b3 12 - six channel servo controller with a PWM output and power line.
Kir 0:d38b3edad9b3 13
Kir 0:d38b3edad9b3 14 The board's i2c interface need to be connected to the PTE0 and PTE1 ports
Kir 0:d38b3edad9b3 15 of the Freescale FRDM-KL25Z device
Kir 0:d38b3edad9b3 16
Kir 0:d38b3edad9b3 17 For more information, please visit our web-site:
Kir 0:d38b3edad9b3 18 *****************************
Kir 0:d38b3edad9b3 19 * http://aupilot.com.au *
Kir 0:d38b3edad9b3 20 *****************************
Kir 0:d38b3edad9b3 21
Kir 0:d38b3edad9b3 22 License:
Kir 0:d38b3edad9b3 23
Kir 0:d38b3edad9b3 24 Permission is hereby granted, free of charge, to any person obtaining a copy
Kir 0:d38b3edad9b3 25 of this software and associated documentation files (the "Software"), to deal
Kir 0:d38b3edad9b3 26 in the Software without restriction, including without limitation the rights
Kir 0:d38b3edad9b3 27 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
Kir 0:d38b3edad9b3 28 copies of the Software, and to permit persons to whom the Software is
Kir 0:d38b3edad9b3 29 furnished to do so, subject to the following conditions:
Kir 0:d38b3edad9b3 30
Kir 0:d38b3edad9b3 31 1) The Software or its substantial portions are used for auPilot product(s);
Kir 0:d38b3edad9b3 32 2) The above copyright notice and this permission notice shall be included in
Kir 0:d38b3edad9b3 33 all copies or substantial portions of the Software.
Kir 0:d38b3edad9b3 34
Kir 0:d38b3edad9b3 35 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
Kir 0:d38b3edad9b3 36 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
Kir 0:d38b3edad9b3 37 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
Kir 0:d38b3edad9b3 38 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
Kir 0:d38b3edad9b3 39 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
Kir 0:d38b3edad9b3 40 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
Kir 0:d38b3edad9b3 41 THE SOFTWARE.
Kir 0:d38b3edad9b3 42
Kir 0:d38b3edad9b3 43 */
Kir 0:d38b3edad9b3 44
Kir 0:d38b3edad9b3 45 // ########## WARNING:
Kir 0:d38b3edad9b3 46 // This test will turn ON the motor. Please make sure that it is SAFE
Kir 0:d38b3edad9b3 47 // ###################
Kir 0:d38b3edad9b3 48 // Test 1: readout the firmware version and status
Kir 0:d38b3edad9b3 49 // Test 2: servo controller test
Kir 0:d38b3edad9b3 50 // Test 3: the main motor(s) test
Kir 0:d38b3edad9b3 51
Kir 0:d38b3edad9b3 52 #include "mbed.h"
Kir 0:d38b3edad9b3 53 #include "itoa.h"
Kir 0:d38b3edad9b3 54
Kir 0:d38b3edad9b3 55 #define AUSPEED_I2C_ADDRESS (0x12<<1)
Kir 0:d38b3edad9b3 56 #define BUFF_SIZE 16
Kir 0:d38b3edad9b3 57
Kir 0:d38b3edad9b3 58 //DigitalOut myled(LED_BLUE);
Kir 0:d38b3edad9b3 59 Serial pc(USBTX, USBRX);
Kir 0:d38b3edad9b3 60 I2C i2c(PTE0, PTE1);
Kir 0:d38b3edad9b3 61
Kir 0:d38b3edad9b3 62 //int i2c_reg8_write(int i2c_address, char reg, const char wd);
Kir 0:d38b3edad9b3 63 int i2c_reg_read(int i2c_address, char reg, char* rd, int len);
Kir 0:d38b3edad9b3 64 void diag(void);
Kir 0:d38b3edad9b3 65
Kir 0:d38b3edad9b3 66
Kir 0:d38b3edad9b3 67 int address = AUSPEED_I2C_ADDRESS;
Kir 0:d38b3edad9b3 68
Kir 0:d38b3edad9b3 69 typedef union _conv_float_bytes {
Kir 0:d38b3edad9b3 70 float f;
Kir 0:d38b3edad9b3 71 uint8_t b[4];
Kir 0:d38b3edad9b3 72 } convert_float2bytes;
Kir 0:d38b3edad9b3 73 convert_float2bytes q;
Kir 0:d38b3edad9b3 74
Kir 0:d38b3edad9b3 75 int motors = 0;
Kir 0:d38b3edad9b3 76
Kir 0:d38b3edad9b3 77 int main()
Kir 0:d38b3edad9b3 78 {
Kir 0:d38b3edad9b3 79 PwmOut led(LED_GREEN);
Kir 0:d38b3edad9b3 80
Kir 0:d38b3edad9b3 81 char rd[BUFF_SIZE];
Kir 0:d38b3edad9b3 82 int slider = 127;
Kir 0:d38b3edad9b3 83 int sliderIncrement = 1;
Kir 0:d38b3edad9b3 84
Kir 0:d38b3edad9b3 85 i2c.frequency(400000);
Kir 0:d38b3edad9b3 86 led = 1;
Kir 0:d38b3edad9b3 87
Kir 0:d38b3edad9b3 88 pc.printf("\n\r\n\rTest program for the auSpeed board");
Kir 0:d38b3edad9b3 89
Kir 0:d38b3edad9b3 90 // === Test 1 ====
Kir 0:d38b3edad9b3 91 pc.printf("\n\rStarting Test 1...");
Kir 0:d38b3edad9b3 92 // read firmware version
Kir 0:d38b3edad9b3 93 if (i2c_reg_read(address, 0x34, rd, 16)) {
Kir 0:d38b3edad9b3 94 pc.printf("\n\ri2c error 01");
Kir 0:d38b3edad9b3 95 memset(rd,0,BUFF_SIZE); // clear the buffer
Kir 0:d38b3edad9b3 96 } else {
Kir 0:d38b3edad9b3 97 char serial[13]= {0};
Kir 0:d38b3edad9b3 98 memcpy(serial,rd,12);
Kir 0:d38b3edad9b3 99 motors = (int)rd[14];
Kir 0:d38b3edad9b3 100 pc.printf("\n\rSerial: %s, Version: %d.%.2d, Motors detected: %d", serial, rd[12], rd[13],motors );
Kir 0:d38b3edad9b3 101 }
Kir 0:d38b3edad9b3 102 wait(1);
Kir 0:d38b3edad9b3 103
Kir 0:d38b3edad9b3 104 // === Test 2 ====
Kir 0:d38b3edad9b3 105 pc.printf("\n\rStarting Test 2...");
Kir 0:d38b3edad9b3 106 pc.printf("\n\rSwinging all servos");
Kir 0:d38b3edad9b3 107 for (int i=0; i<2; i++) {
Kir 0:d38b3edad9b3 108 for (int j=0; j<250; j++) {
Kir 0:d38b3edad9b3 109 rd[0] = 0x21;
Kir 0:d38b3edad9b3 110 rd[1] = (char) slider;
Kir 0:d38b3edad9b3 111 rd[2] = (char) slider;
Kir 0:d38b3edad9b3 112 rd[3] = (char) slider;
Kir 0:d38b3edad9b3 113 rd[4] = (char) slider;
Kir 0:d38b3edad9b3 114 rd[5] = (char) slider;
Kir 0:d38b3edad9b3 115 rd[6] = (char) slider;
Kir 0:d38b3edad9b3 116 if (i2c.write(address, rd, 7, false) == 0)
Kir 0:d38b3edad9b3 117 ; // pc.printf("\n\rSuccess");
Kir 0:d38b3edad9b3 118 else
Kir 0:d38b3edad9b3 119 pc.printf("\n\ri2c error 02");
Kir 0:d38b3edad9b3 120 led = (float)slider / 256;
Kir 0:d38b3edad9b3 121 wait(0.01);
Kir 0:d38b3edad9b3 122 slider += sliderIncrement;
Kir 0:d38b3edad9b3 123 if (slider > 250)
Kir 0:d38b3edad9b3 124 sliderIncrement = -2;
Kir 0:d38b3edad9b3 125 if (slider < 2)
Kir 0:d38b3edad9b3 126 sliderIncrement = +2;
Kir 0:d38b3edad9b3 127 }
Kir 0:d38b3edad9b3 128 }
Kir 0:d38b3edad9b3 129 led = 1;
Kir 0:d38b3edad9b3 130
Kir 0:d38b3edad9b3 131 // === Test 3 ====
Kir 0:d38b3edad9b3 132 pc.printf("\n\rStarting Test 3...");
Kir 0:d38b3edad9b3 133 pc.printf("\n\rWARNING Motor will be started!");
Kir 0:d38b3edad9b3 134 wait(1);
Kir 0:d38b3edad9b3 135
Kir 0:d38b3edad9b3 136 for (int i=0; i<3; i++) {
Kir 0:d38b3edad9b3 137 // ========= start ========
Kir 0:d38b3edad9b3 138 rd[0] = 0x01;
Kir 0:d38b3edad9b3 139 rd[1] = 0x30;
Kir 0:d38b3edad9b3 140 if (i2c.write(address, rd, 2) != 0)
Kir 0:d38b3edad9b3 141 pc.printf("\n\ri2c error 03");
Kir 0:d38b3edad9b3 142 wait(1);
Kir 0:d38b3edad9b3 143 diag();
Kir 0:d38b3edad9b3 144
Kir 0:d38b3edad9b3 145 // ====== accelerate ======
Kir 0:d38b3edad9b3 146 rd[0] = 0x01;
Kir 0:d38b3edad9b3 147 rd[1] = 0x40;
Kir 0:d38b3edad9b3 148 if (i2c.write(address, rd, 2) != 0)
Kir 0:d38b3edad9b3 149 pc.printf("\n\ri2c error 04");
Kir 0:d38b3edad9b3 150 wait(1);
Kir 0:d38b3edad9b3 151 diag(); // this might show soft fail due to sync lost when accelerate
Kir 0:d38b3edad9b3 152
Kir 0:d38b3edad9b3 153 // ========= stop =========
Kir 0:d38b3edad9b3 154 rd[0] = 0x05;
Kir 0:d38b3edad9b3 155 rd[1] = 0x01;
Kir 0:d38b3edad9b3 156 if (i2c.write(address, rd, 2) != 0)
Kir 0:d38b3edad9b3 157 pc.printf("\n\ri2c error 05");
Kir 0:d38b3edad9b3 158 wait(1);
Kir 0:d38b3edad9b3 159 diag();
Kir 0:d38b3edad9b3 160 }
Kir 0:d38b3edad9b3 161 pc.printf("\n\rAll tests done!");
Kir 0:d38b3edad9b3 162 }
Kir 0:d38b3edad9b3 163
Kir 0:d38b3edad9b3 164
Kir 0:d38b3edad9b3 165 // reads 1 byte from a register of an i2c device
Kir 0:d38b3edad9b3 166 // return: rd data read from the register
Kir 0:d38b3edad9b3 167 // return: 0 if OK, -1 otherwise
Kir 0:d38b3edad9b3 168 int i2c_reg_read(int i2c_address, char reg, char* rd, int len)
Kir 0:d38b3edad9b3 169 {
Kir 0:d38b3edad9b3 170 wait(0.001); // we need to keep the command flow under 1000 per second, otherwise they might get skipped.
Kir 0:d38b3edad9b3 171 if (i2c.write(i2c_address, &reg, 1, true) == 0)
Kir 0:d38b3edad9b3 172 if (i2c.read(i2c_address, rd, len) == 0) {
Kir 0:d38b3edad9b3 173 i2c.stop();
Kir 0:d38b3edad9b3 174 return 0;
Kir 0:d38b3edad9b3 175 }
Kir 0:d38b3edad9b3 176 i2c.stop();
Kir 0:d38b3edad9b3 177 return -1;
Kir 0:d38b3edad9b3 178 }
Kir 0:d38b3edad9b3 179
Kir 0:d38b3edad9b3 180 // read and print RPMs and battery data
Kir 0:d38b3edad9b3 181 void diag(void)
Kir 0:d38b3edad9b3 182 {
Kir 0:d38b3edad9b3 183 char rd[BUFF_SIZE] = {0};
Kir 0:d38b3edad9b3 184 float main_battery, spare_battery, current, temperature, work;
Kir 0:d38b3edad9b3 185 int rpms=0;
Kir 0:d38b3edad9b3 186 char status[9]={0};
Kir 0:d38b3edad9b3 187
Kir 0:d38b3edad9b3 188 main_battery =spare_battery =current = temperature= work=0;
Kir 0:d38b3edad9b3 189
Kir 0:d38b3edad9b3 190 // == read rpms of the 1st motor
Kir 0:d38b3edad9b3 191 if (i2c_reg_read(address, 0x01, rd, 4)) {
Kir 0:d38b3edad9b3 192 pc.printf("\n\ri2c error 10");
Kir 0:d38b3edad9b3 193 memset(rd,0,BUFF_SIZE); // clear the buffer
Kir 0:d38b3edad9b3 194 } else {
Kir 0:d38b3edad9b3 195 rpms = (uint32_t)rd[0]+((uint32_t)rd[1])*256+((uint32_t)rd[2])*256*256+((uint32_t)rd[3])*256*256*256;
Kir 0:d38b3edad9b3 196 }
Kir 0:d38b3edad9b3 197 // == read status of the 1st motor
Kir 0:d38b3edad9b3 198 if (i2c_reg_read(address, 0x31, rd, 1)) {
Kir 0:d38b3edad9b3 199 pc.printf("\n\ri2c error 11");
Kir 0:d38b3edad9b3 200 memset(rd,0,BUFF_SIZE); // clear the buffer
Kir 0:d38b3edad9b3 201 } else {
Kir 0:d38b3edad9b3 202 itoa(rd[0],status,2);
Kir 0:d38b3edad9b3 203 }
Kir 0:d38b3edad9b3 204 // == read main battery
Kir 0:d38b3edad9b3 205 if (i2c_reg_read(address, 0x50, rd, 4)) {
Kir 0:d38b3edad9b3 206 pc.printf("\n\ri2c error 12");
Kir 0:d38b3edad9b3 207 memset(rd,0,BUFF_SIZE); // clear the buffer
Kir 0:d38b3edad9b3 208 } else {
Kir 0:d38b3edad9b3 209 memcpy(q.b, rd,4);
Kir 0:d38b3edad9b3 210 main_battery = q.f;
Kir 0:d38b3edad9b3 211 }
Kir 0:d38b3edad9b3 212 // == read spare battery
Kir 0:d38b3edad9b3 213 if (i2c_reg_read(address, 0x51, rd, 4)) {
Kir 0:d38b3edad9b3 214 pc.printf("\n\ri2c error 13");
Kir 0:d38b3edad9b3 215 memset(rd,0,BUFF_SIZE); // clear the buffer
Kir 0:d38b3edad9b3 216 } else {
Kir 0:d38b3edad9b3 217 memcpy(q.b, rd,4);
Kir 0:d38b3edad9b3 218 spare_battery = q.f;
Kir 0:d38b3edad9b3 219 }
Kir 0:d38b3edad9b3 220 // == read current
Kir 0:d38b3edad9b3 221 if (i2c_reg_read(address, 0x52, rd, 4)) {
Kir 0:d38b3edad9b3 222 pc.printf("\n\ri2c error 14");
Kir 0:d38b3edad9b3 223 memset(rd,0,BUFF_SIZE); // clear the buffer
Kir 0:d38b3edad9b3 224 } else {
Kir 0:d38b3edad9b3 225 memcpy(q.b, rd,4);
Kir 0:d38b3edad9b3 226 current = q.f;
Kir 0:d38b3edad9b3 227 }
Kir 0:d38b3edad9b3 228 // == read electrical work
Kir 0:d38b3edad9b3 229 if (i2c_reg_read(address, 0x54, rd, 4)) {
Kir 0:d38b3edad9b3 230 pc.printf("\n\ri2c error 15");
Kir 0:d38b3edad9b3 231 memset(rd,0,BUFF_SIZE); // clear the buffer
Kir 0:d38b3edad9b3 232 } else {
Kir 0:d38b3edad9b3 233 memcpy(q.b, rd,4);
Kir 0:d38b3edad9b3 234 work = q.f;
Kir 0:d38b3edad9b3 235 }
Kir 0:d38b3edad9b3 236 // == read temperature
Kir 0:d38b3edad9b3 237 if (i2c_reg_read(address, 0x53, rd,4)) {
Kir 0:d38b3edad9b3 238 pc.printf("\n\ri2c error 16");
Kir 0:d38b3edad9b3 239 memset(rd,0,BUFF_SIZE); // clear the buffer
Kir 0:d38b3edad9b3 240 } else {
Kir 0:d38b3edad9b3 241 memcpy(q.b, rd,4);
Kir 0:d38b3edad9b3 242 temperature = q.f;
Kir 0:d38b3edad9b3 243 }
Kir 0:d38b3edad9b3 244 pc.printf("\n\rS %08s RPMs: %.6d Main: %.2fV Spare: %.2fV Current %.0fmA Work %.2f Temp %.1fC",
Kir 0:d38b3edad9b3 245 status,rpms,main_battery,spare_battery,current,work,temperature);
Kir 0:d38b3edad9b3 246 }