DAISEN eDES see: http://mbed.org/users/okini3939/notebook/daisen-edes/
eDES2WD.cpp@2:ef1c45486022, 2013-06-17 (annotated)
- Committer:
- okini3939
- Date:
- Mon Jun 17 01:46:35 2013 +0000
- Revision:
- 2:ef1c45486022
- Parent:
- 1:2a38c9027bd1
fix;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
okini3939 | 1:2a38c9027bd1 | 1 | /** |
okini3939 | 1:2a38c9027bd1 | 2 | * DAISEN eDES 2WD driver for mbed |
okini3939 | 1:2a38c9027bd1 | 3 | * Copyright (c) 2012 Suga |
okini3939 | 1:2a38c9027bd1 | 4 | * Released under the MIT License: http://mbed.org/license/mit |
okini3939 | 1:2a38c9027bd1 | 5 | */ |
okini3939 | 1:2a38c9027bd1 | 6 | |
okini3939 | 1:2a38c9027bd1 | 7 | /** @file |
okini3939 | 1:2a38c9027bd1 | 8 | * @brief DAISEN eDES 2WD driver for mbed |
okini3939 | 1:2a38c9027bd1 | 9 | */ |
okini3939 | 1:2a38c9027bd1 | 10 | |
okini3939 | 1:2a38c9027bd1 | 11 | #include "mbed.h" |
okini3939 | 1:2a38c9027bd1 | 12 | #include "eDES2WD.h" |
okini3939 | 1:2a38c9027bd1 | 13 | |
okini3939 | 1:2a38c9027bd1 | 14 | eDES2WD::eDES2WD (I2C &i2c) : _i2c(i2c) { |
okini3939 | 1:2a38c9027bd1 | 15 | } |
okini3939 | 1:2a38c9027bd1 | 16 | |
okini3939 | 1:2a38c9027bd1 | 17 | void eDES2WD::reset (int id) { |
okini3939 | 1:2a38c9027bd1 | 18 | char cmd[] = {1, I2C_WCMD_RESET}; |
okini3939 | 1:2a38c9027bd1 | 19 | int adrs; |
okini3939 | 1:2a38c9027bd1 | 20 | |
okini3939 | 1:2a38c9027bd1 | 21 | if (0 < id){ |
okini3939 | 1:2a38c9027bd1 | 22 | adrs = SUB_ADRS + (id - 1) * 2; |
okini3939 | 1:2a38c9027bd1 | 23 | } else { |
okini3939 | 1:2a38c9027bd1 | 24 | adrs = 0; |
okini3939 | 1:2a38c9027bd1 | 25 | } |
okini3939 | 1:2a38c9027bd1 | 26 | _i2c.write(adrs, cmd, 2); |
okini3939 | 1:2a38c9027bd1 | 27 | } |
okini3939 | 1:2a38c9027bd1 | 28 | |
okini3939 | 1:2a38c9027bd1 | 29 | void eDES2WD::motor (int id, int l_speed, int r_speed) { |
okini3939 | 1:2a38c9027bd1 | 30 | char cmd[] = {3, I2C_WCMD_MOTOR, 0, 0}; |
okini3939 | 1:2a38c9027bd1 | 31 | int adrs; |
okini3939 | 1:2a38c9027bd1 | 32 | |
okini3939 | 1:2a38c9027bd1 | 33 | cmd[2] = l_speed; |
okini3939 | 1:2a38c9027bd1 | 34 | cmd[3] = r_speed; |
okini3939 | 1:2a38c9027bd1 | 35 | if (0 < id){ |
okini3939 | 1:2a38c9027bd1 | 36 | adrs = SUB_ADRS + (id - 1) * 2; |
okini3939 | 1:2a38c9027bd1 | 37 | } else { |
okini3939 | 1:2a38c9027bd1 | 38 | adrs = 0; |
okini3939 | 1:2a38c9027bd1 | 39 | } |
okini3939 | 1:2a38c9027bd1 | 40 | _i2c.write(adrs, cmd, 4); |
okini3939 | 1:2a38c9027bd1 | 41 | } |
okini3939 | 1:2a38c9027bd1 | 42 | |
okini3939 | 1:2a38c9027bd1 | 43 | void eDES2WD::set_led (int id, eDES2WD_LED led, int sw) { |
okini3939 | 1:2a38c9027bd1 | 44 | char cmd[] = {3, I2C_WCMD_LED, 0, 0}; |
okini3939 | 1:2a38c9027bd1 | 45 | int adrs; |
okini3939 | 1:2a38c9027bd1 | 46 | |
okini3939 | 1:2a38c9027bd1 | 47 | cmd[2] = led; |
okini3939 | 1:2a38c9027bd1 | 48 | cmd[3] = sw; |
okini3939 | 1:2a38c9027bd1 | 49 | if (0 < id){ |
okini3939 | 1:2a38c9027bd1 | 50 | adrs = SUB_ADRS + (id - 1) * 2; |
okini3939 | 1:2a38c9027bd1 | 51 | } else { |
okini3939 | 1:2a38c9027bd1 | 52 | adrs = 0; |
okini3939 | 1:2a38c9027bd1 | 53 | } |
okini3939 | 1:2a38c9027bd1 | 54 | _i2c.write(adrs, cmd, 4); |
okini3939 | 1:2a38c9027bd1 | 55 | } |
okini3939 | 1:2a38c9027bd1 | 56 | |
okini3939 | 2:ef1c45486022 | 57 | unsigned int eDES2WD::get_sensor(int id, eDES2WD_SENSOR cn, unsigned int *dat) { |
okini3939 | 1:2a38c9027bd1 | 58 | char cmd[] = {2, I2C_RCMD_SENSOR, 0}; |
okini3939 | 1:2a38c9027bd1 | 59 | char recv[MAX_CN]; |
okini3939 | 1:2a38c9027bd1 | 60 | unsigned int d; |
okini3939 | 1:2a38c9027bd1 | 61 | int adrs; |
okini3939 | 1:2a38c9027bd1 | 62 | int nmax, n; |
okini3939 | 1:2a38c9027bd1 | 63 | |
okini3939 | 1:2a38c9027bd1 | 64 | if (id < 1 || 9 < id) return(0); |
okini3939 | 1:2a38c9027bd1 | 65 | adrs = SUB_ADRS + (id - 1) * 2; |
okini3939 | 1:2a38c9027bd1 | 66 | nmax = cn < MAX_CN ? 2 : MAX_CN * 2 + 2; |
okini3939 | 1:2a38c9027bd1 | 67 | |
okini3939 | 1:2a38c9027bd1 | 68 | cmd[2] = cn; |
okini3939 | 1:2a38c9027bd1 | 69 | _i2c.write(adrs, cmd, 3); |
okini3939 | 1:2a38c9027bd1 | 70 | _i2c.read(adrs | 1, recv, nmax); |
okini3939 | 1:2a38c9027bd1 | 71 | if (nmax == 2){ |
okini3939 | 1:2a38c9027bd1 | 72 | d = recv[0] | (recv[1] << 8); |
okini3939 | 1:2a38c9027bd1 | 73 | return d; |
okini3939 | 1:2a38c9027bd1 | 74 | } else { |
okini3939 | 1:2a38c9027bd1 | 75 | for (n = 0; n < MAX_CN; n++){ |
okini3939 | 1:2a38c9027bd1 | 76 | d = recv[0] | (recv[1] << 8); |
okini3939 | 1:2a38c9027bd1 | 77 | dat[n] = d; |
okini3939 | 1:2a38c9027bd1 | 78 | } |
okini3939 | 1:2a38c9027bd1 | 79 | // gSUB_LED_G = recv[20]; |
okini3939 | 1:2a38c9027bd1 | 80 | // gSUB_LED_R = recv[21]; |
okini3939 | 1:2a38c9027bd1 | 81 | } |
okini3939 | 1:2a38c9027bd1 | 82 | return 0; |
okini3939 | 1:2a38c9027bd1 | 83 | } |
okini3939 | 1:2a38c9027bd1 | 84 | |
okini3939 | 1:2a38c9027bd1 | 85 | int eDES2WD::get_lat(int id, int cno) { |
okini3939 | 1:2a38c9027bd1 | 86 | char cmd[] = {2, I2C_RCMD_LAT, 0}; |
okini3939 | 1:2a38c9027bd1 | 87 | char recv[MAX_CN]; |
okini3939 | 1:2a38c9027bd1 | 88 | int adrs; |
okini3939 | 1:2a38c9027bd1 | 89 | |
okini3939 | 1:2a38c9027bd1 | 90 | if (id < 1 || 9 < id) return(false); |
okini3939 | 1:2a38c9027bd1 | 91 | if (MAX_CN - 1 < cno) return(false); |
okini3939 | 1:2a38c9027bd1 | 92 | adrs = SUB_ADRS + (id - 1) * 2; |
okini3939 | 1:2a38c9027bd1 | 93 | |
okini3939 | 1:2a38c9027bd1 | 94 | cmd[2] = cno; |
okini3939 | 1:2a38c9027bd1 | 95 | _i2c.write(adrs, cmd, 3); |
okini3939 | 1:2a38c9027bd1 | 96 | _i2c.read(adrs | 1, recv, 1); |
okini3939 | 1:2a38c9027bd1 | 97 | return recv[0]; |
okini3939 | 1:2a38c9027bd1 | 98 | } |