Suga koubou / eDES2WD

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?

UserRevisionLine numberNew 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 }