Mato Hattori
rev.0.0
Dependencies: EthernetInterface mbed-rtos mbed
Revision 0:f773795ad34d, committed 2017-11-13
- Comitter:
- matohatto
- Date:
- Mon Nov 13 06:51:27 2017 +0000
- Commit message:
- for RaccoonII rev.0, the Motor Driver for model ship.
; Engineering faculty, Osaka university, Osaka, Japan.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EthernetInterface.lib Mon Nov 13 06:51:27 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/EthernetInterface/#183490eb1b4a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Nov 13 06:51:27 2017 +0000
@@ -0,0 +1,221 @@
+/////////////////////////////////////////////////////
+// Motor Driver mbed microcontroller firmware
+// Version 1.0
+//
+// Copyright (C) 2017 - All rights reserved
+// Base Address Technologies Japan, Ltd.
+//
+// Created : Oct.18th, 2017 by Mato Hattori
+// Modified : Nov.13th, 2017 by Mato Hattori Added socket timeout
+/////////////////////////////////////////////////////
+
+// Header files
+#include "mbed.h"
+#include "EthernetInterface.h"
+#include <PwmOut.h>
+
+// Macro definition
+#define MAX_THROTTLE_PWMDUTY 0.99 // maximum duty clcle for PWM output when 100% throttle
+#define LOOP_INTERVAL_MILISEC 100 // loop interval in mili-seconds
+#define UDP_TIMEOUT_MILISEC 1000 // UDP socket timeout in mili-seconds
+#define MSEC /1000.0 // factor for wait argument, /10000.0 when using mbed-os version 5.6
+//#define I2C_ADDR (0x01) // CPLD I2C address, I2C not used for now
+
+// Ethernet Configuration data
+char NetMask[] = "255.255.255.0";
+char GatewayAddr[] = "192.168.11.1";
+char HostAddr[] = "192.168.11.9"; // no effect bug ???
+int HostPort = 4001;
+char RCReceiverAddr[] = "192.168.11.240"; // ignored ?? bug???
+int RCReceiverPort = 4001;
+char MotorDriverAddr[] = "192.168.11.242";
+int MotorDriverPort = 4000;
+
+// Hardware Class instances
+DigitalOut myled (LED1); // run indicator
+//I2C i2c (p9, p10); // pulse counter PLD, I2C not used for now
+DigitalInOut sda (p9); // TODO change this to I2C
+DigitalOut scl (p10); // TODO change this to I2C
+//DigitalOut mP (p26); // TODO change this to I2C
+//DigitalOut mN (p25); // TODO change this to I2C
+PwmOut motor_P (p26); // motor left positive
+PwmOut motor_N (p25); // motor left negative
+AnalogIn curr_raw (p16); // current sensor raw digital
+AnalogIn temp_raw (p17); // temperature sensor raw digital
+EthernetInterface eth; // ethernet interface
+
+// Function plototypes
+int pulse (void);
+float current (void);
+float temperature (void);
+
+// Main
+int main()
+{
+ char buf[256];
+ int ret, cnt, rps, i;
+ unsigned long tick = 0;
+ float cur, tmp;
+ int udpin_buffer[2] = {0, 0,};
+// int udpout_buffer[3];
+ int throttle = 0;
+ float pwmduty = 0.0;
+
+ // Initializations
+ //i2c.frequency (100000); // TODO
+ motor_P.period (0.01);
+ motor_N.period (0.01);
+ motor_P = 0.5;
+ motor_N = 0.5;
+
+ // Ethernet Init
+ EthernetInterface eth;
+ if(ret = eth.init (MotorDriverAddr, NetMask, GatewayAddr))
+ {
+ printf ("Ethernet init failed : ret=%d", ret);
+ return -1;
+ }
+ eth.connect();
+ printf("Ethernet Connected\n");
+ printf("------------------------------\n");
+ printf("IP Address is %s\n",eth.getIPAddress());
+ printf("Net Mask is %s\n",eth.getNetworkMask());
+ printf("------------------------------\n");
+
+ // UDP Init
+ UDPSocket sock;
+ sock.bind (MotorDriverPort);
+ sock.set_blocking (false, 500); // Set non-blobking
+
+ // RC Rx Endpoint Init
+ Endpoint rcrx;
+ rcrx.set_address (RCReceiverAddr, RCReceiverPort);
+
+ // Host Endpoint Init
+ Endpoint host;
+ host.set_address (HostAddr, HostPort);
+
+ // Main Routine
+ myled = 0; // turn off LED
+
+ while(1)
+ {
+ // UDP Communication Sequence
+ printf ("rx -> md : ");
+ memset (udpin_buffer, 4, 0);
+ ret = sock.receiveFrom (rcrx, (char *)udpin_buffer, 4);
+ printf ("ret=%d : %d %d\n", ret, udpin_buffer[0], udpin_buffer[1]); // DEBUG
+
+ if (ret <= 0)
+ {
+ // No valid data received
+ throttle= 0;
+ }
+ else if ((udpin_buffer[1] & 0x03) == 0)
+ {
+ // Manual mode
+ throttle = udpin_buffer[0];
+ }
+ else if ((udpin_buffer[1] & 0x03) == 1)
+ {
+ // Auto mode
+ printf ("pc -> md : ");
+ memset (udpin_buffer, 4, 0);
+ sock.receiveFrom (host, (char *)udpin_buffer, 4);
+ printf ("%d %d\n", udpin_buffer[0], udpin_buffer[1]); // DEBUG
+
+ throttle = udpin_buffer[0];
+ }
+
+ pwmduty = MAX_THROTTLE_PWMDUTY * throttle / 100.0 / 2.0; // pwmduty in range of +-0.5
+
+ if (pwmduty > 0.49) // sanity check
+ pwmduty = 0.49;
+ else if (pwmduty < -0.49)
+ pwmduty = -0.49;
+
+ motor_P = (0.5 + pwmduty);
+ motor_N = (0.5 - pwmduty);
+
+
+ rps = pulse ();
+ cur = current ();
+ tmp = temperature ();
+ printf ("#%ld : rps=%d\tcurr=%f\ttemp=%f\n", tick++, rps, cur, tmp); // print tick then increment
+
+// udpout_buffer[0] = pul;
+// udpout_buffer[1] = (int)(cur+0.5);
+// udpout_buffer[2] = (int)(tmp+0.5);
+// sock.sendTo (rcrx, (char *)udpout_buffer, sizeof (udpout_buffer));
+// printf ("tx -> %d %d %d\n", udpout_buffer[0], udpout_buffer[1], udpout_buffer[2]); // DEBUG
+
+
+ // DEBUG DEBUG DEBUG
+ myled = !myled; // invert LED
+ if (ret > 0) // wait a bit when UDP success
+ wait (LOOP_INTERVAL_MILISEC MSEC);
+ }
+
+ myled = 0; // turn off LED
+ sock.close();
+ eth.disconnect();
+}
+
+// pulse ()
+// returns int pulse count per 0.25sec
+int pulse (void)
+{
+ int i;
+ int cnt = 0;
+
+ //printf ("reading pulse count...\n"); // DEBUG
+ //i2c.start (); // TODO change to I2C interface
+ //i2c.read (I2C_ADDR, (char *)(&cnt), 2, 0);
+ //i2c.stop ();
+ scl = 0; // psuedo i2c
+ sda = 1;
+ sda.output ();
+ wait (0.01 MSEC);
+ scl = 1;
+ wait (0.01 MSEC);
+ sda = 0; // start condition
+ wait (0.01 MSEC);
+ scl = 0;
+ sda.input ();
+ for (i=0; i<16; i++)
+ {
+ scl = 1;
+ wait (0.01 MSEC);
+ cnt = (cnt << 1) | sda;
+ scl = 0;
+ wait (0.01 MSEC);
+ }
+ scl = 0;
+ sda = 0;
+ sda.output ();
+ wait (0.01 MSEC);
+ scl = 1;
+ wait (0.01 MSEC);
+ sda = 1; // stop condition
+ wait (0.01 MSEC);
+ scl = 0;
+ sda = 0;
+ sda.input ();
+
+ return ( cnt );
+}
+
+// current ()
+// returns float in range of +-100.0 Ampere
+float current (void)
+{
+ return ( ((int)(((curr_raw * 3.3 - 1.644) * 100.0) * 10.0)) / 10.0 );
+}
+
+// temperature ()
+// returns float in range of -20.0 to +100.0 degree Celsius
+float temperature (void)
+{
+ return ( ((int)(((temp_raw *3.3 - 0.89) * 43.0) * 10.0)) / 10.0 );
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Mon Nov 13 06:51:27 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed-rtos/#5713cbbdb706
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Nov 13 06:51:27 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/e7ca05fa8600 \ No newline at end of file