Diff: main.cpp

Revision:

0:d38b3edad9b3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 17 08:46:26 2015 +0000
@@ -0,0 +1,246 @@
+/*
+        Copyright (c) 2015 Aurobo Pty Ltd
+
+=== A test program for the auSpeed Integrated Combo board ===
+                    Version 1.00
+
+The auSpeed-1M combo controller is an integrated module that combines 
+three UAV sub-systems:
+- high efficient protected power supply for electronic units and servos 
+  with voltage and current sensors (BEC);
+- brushless motor speed controller (ESC), up to 40A;
+- six channel servo controller with a PWM output and power line.
+
+The board's i2c interface need to be connected to the PTE0 and PTE1 ports 
+    of the Freescale FRDM-KL25Z device
+
+For more information, please visit our web-site:
+            *****************************
+            *   http://aupilot.com.au   *
+            *****************************
+
+License:
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+1) The Software or its substantial portions are used for auPilot product(s);
+2) The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+
+*/
+
+// ########## WARNING: 
+//             This test will turn ON the motor. Please make sure that it is SAFE
+// ###################
+// Test 1:  readout the firmware version and status
+// Test 2:  servo controller test
+// Test 3:  the main motor(s) test 
+
+#include "mbed.h"
+#include "itoa.h"
+
+#define AUSPEED_I2C_ADDRESS (0x12<<1)
+#define BUFF_SIZE 16
+
+//DigitalOut myled(LED_BLUE);
+Serial pc(USBTX, USBRX);
+I2C i2c(PTE0, PTE1);
+
+//int i2c_reg8_write(int i2c_address, char reg, const char wd);
+int i2c_reg_read(int i2c_address, char reg, char* rd, int len);
+void diag(void);
+
+
+int address = AUSPEED_I2C_ADDRESS;
+
+typedef union _conv_float_bytes {
+    float f;
+    uint8_t b[4];
+} convert_float2bytes;
+convert_float2bytes q;
+
+int motors = 0;
+
+int main()
+{
+    PwmOut led(LED_GREEN);
+
+    char   rd[BUFF_SIZE];
+    int    slider = 127;
+    int    sliderIncrement = 1;
+
+    i2c.frequency(400000);
+    led = 1;
+
+    pc.printf("\n\r\n\rTest program for the auSpeed board");
+
+    // === Test 1 ====
+    pc.printf("\n\rStarting Test 1...");
+    // read firmware version
+    if (i2c_reg_read(address, 0x34, rd, 16)) {
+        pc.printf("\n\ri2c error 01");
+        memset(rd,0,BUFF_SIZE); // clear the buffer
+    } else {
+        char serial[13]= {0};
+        memcpy(serial,rd,12);
+        motors = (int)rd[14];
+        pc.printf("\n\rSerial: %s, Version: %d.%.2d, Motors detected: %d", serial, rd[12], rd[13],motors );
+    }
+    wait(1);
+
+    // === Test 2 ====
+    pc.printf("\n\rStarting Test 2...");
+    pc.printf("\n\rSwinging all servos");
+    for (int i=0; i<2; i++) {
+        for (int j=0; j<250; j++) {
+            rd[0] = 0x21;
+            rd[1] = (char) slider;
+            rd[2] = (char) slider;
+            rd[3] = (char) slider;
+            rd[4] = (char) slider;
+            rd[5] = (char) slider;
+            rd[6] = (char) slider;
+            if (i2c.write(address, rd, 7, false) == 0)
+                ; // pc.printf("\n\rSuccess");
+            else
+                pc.printf("\n\ri2c error 02");
+            led = (float)slider / 256;
+            wait(0.01);
+            slider += sliderIncrement;
+            if (slider > 250)
+                sliderIncrement = -2;
+            if (slider < 2)
+                sliderIncrement = +2;
+        }
+    }
+    led = 1;
+
+    // === Test 3 ====
+    pc.printf("\n\rStarting Test 3...");
+    pc.printf("\n\rWARNING Motor will be started!");
+    wait(1);
+
+    for (int i=0; i<3; i++) {
+        // ========= start ========
+        rd[0] = 0x01;
+        rd[1] = 0x30;
+        if (i2c.write(address, rd, 2) != 0)
+            pc.printf("\n\ri2c error 03");
+        wait(1);
+        diag();
+
+        // ====== accelerate ======
+        rd[0] = 0x01;
+        rd[1] = 0x40;
+        if (i2c.write(address, rd, 2) != 0)
+            pc.printf("\n\ri2c error 04");
+        wait(1);
+        diag();         // this might show soft fail due to sync lost when accelerate
+ 
+        // ========= stop =========
+        rd[0] = 0x05;
+        rd[1] = 0x01;
+        if (i2c.write(address, rd, 2) != 0)
+            pc.printf("\n\ri2c error 05");
+        wait(1);
+        diag();
+    }
+    pc.printf("\n\rAll tests done!");
+}
+
+
+// reads 1 byte from a register of an i2c device
+// return: rd data read from the register
+// return: 0 if OK, -1 otherwise
+int i2c_reg_read(int i2c_address, char reg, char* rd, int len)
+{
+    wait(0.001);    // we need to keep the command flow under 1000 per second, otherwise they might get skipped.
+    if (i2c.write(i2c_address, &reg, 1, true) == 0)  
+        if (i2c.read(i2c_address, rd, len) == 0) {
+            i2c.stop();
+            return 0;
+        }
+    i2c.stop();
+    return -1;
+}
+
+// read and print RPMs and battery data
+void diag(void)
+{
+    char  rd[BUFF_SIZE] = {0};
+    float main_battery, spare_battery, current, temperature, work;
+    int   rpms=0;
+    char  status[9]={0};
+
+    main_battery =spare_battery =current = temperature= work=0;
+
+    // == read rpms of the 1st motor
+    if (i2c_reg_read(address, 0x01, rd, 4)) {
+        pc.printf("\n\ri2c error 10");
+        memset(rd,0,BUFF_SIZE); // clear the buffer
+    } else {
+        rpms = (uint32_t)rd[0]+((uint32_t)rd[1])*256+((uint32_t)rd[2])*256*256+((uint32_t)rd[3])*256*256*256;
+    }
+    // == read status of the 1st motor
+    if (i2c_reg_read(address, 0x31, rd, 1)) {
+        pc.printf("\n\ri2c error 11");
+        memset(rd,0,BUFF_SIZE); // clear the buffer
+    } else {
+        itoa(rd[0],status,2);
+    }
+    // == read main battery
+    if (i2c_reg_read(address, 0x50, rd, 4)) {
+        pc.printf("\n\ri2c error 12");
+        memset(rd,0,BUFF_SIZE); // clear the buffer
+    } else {
+        memcpy(q.b, rd,4);
+        main_battery = q.f;
+    }
+    // == read spare battery
+    if (i2c_reg_read(address, 0x51, rd, 4)) {
+        pc.printf("\n\ri2c error 13");
+        memset(rd,0,BUFF_SIZE); // clear the buffer
+    } else {
+        memcpy(q.b, rd,4);
+        spare_battery = q.f;
+    }
+    // == read current
+    if (i2c_reg_read(address, 0x52, rd, 4)) {
+        pc.printf("\n\ri2c error 14");
+        memset(rd,0,BUFF_SIZE); // clear the buffer
+    } else {
+        memcpy(q.b, rd,4);
+        current = q.f;
+    }
+    // == read electrical work
+    if (i2c_reg_read(address, 0x54, rd, 4)) {
+        pc.printf("\n\ri2c error 15");
+        memset(rd,0,BUFF_SIZE); // clear the buffer
+    } else {
+        memcpy(q.b, rd,4);
+        work = q.f;
+    }
+    // == read temperature
+    if (i2c_reg_read(address, 0x53, rd,4)) {
+        pc.printf("\n\ri2c error 16");
+        memset(rd,0,BUFF_SIZE); // clear the buffer
+    } else {
+        memcpy(q.b, rd,4);
+        temperature = q.f;
+    }
+    pc.printf("\n\rS %08s RPMs: %.6d Main: %.2fV Spare: %.2fV Current %.0fmA Work %.2f Temp %.1fC", 
+                    status,rpms,main_battery,spare_battery,current,work,temperature);
+}
\ No newline at end of file