Revision 0:a11da93ea3f6, committed 2013-11-02

Comitter:

jksoft

Date:

Sat Nov 02 01:04:00 2013 +0000

Commit message:

Rev1

Changed in this revision

diff -r 000000000000 -r a11da93ea3f6 TB6612/TB6612.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TB6612/TB6612.cpp	Sat Nov 02 01:04:00 2013 +0000
@@ -0,0 +1,71 @@
+/**
+ * Motor Driver TB6612 Control Library
+ *
+ * -- TB6612 is a device of the TOSHIBA. 
+ *
+ * Copyright (C) 2012 Junichi Katsu (JKSOFT) 
+ */
+
+
+#include "TB6612.h"
+
+// TB6612 Class Constructor
+TB6612::TB6612(PinName pwm, PinName fwd, PinName rev):
+        _pwm(pwm), _fwd(fwd), _rev(rev) {
+
+    _fwd = 0;
+    _rev = 0;
+    _pwm = 0.0;
+    _pwm.period(0.001);
+    
+    bspeed = 0.0;
+    timer_flag = false;
+}
+
+// Speed Control
+//  arg
+//   float speed -1.0 - 0.0 - 1.0
+void TB6612::speed(float speed) {
+    
+    if( timer_flag == true )    return;
+    
+    bspeed = speed;
+    
+    if( speed > 0.0 )
+    {
+        _pwm = speed;
+        _fwd = 1;
+        _rev = 0;
+    }
+    else if( speed < 0.0 )
+    {
+        _pwm = -speed;
+        _fwd = 0;
+        _rev = 1;
+    }
+    else
+    {
+        _fwd = 1;
+        _rev = 1;
+    }
+}
+
+
+// Speed Control with time-out
+//  arg
+//   float sspeed:-1.0 - 0.0 - 1.0
+//   float time  :0.0-
+void TB6612::move(float sspeed , float time)
+{
+    speed(sspeed);
+    timer_flag = true;
+    timer.attach(this,&TB6612::timeout,time);
+}
+
+
+void TB6612::timeout()
+{
+    timer_flag = false;
+    speed(bspeed);
+}
+

diff -r 000000000000 -r a11da93ea3f6 TB6612/TB6612.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TB6612/TB6612.h	Sat Nov 02 01:04:00 2013 +0000
@@ -0,0 +1,35 @@
+/**
+ * Motor Driver TB6612 Control Library
+ *
+ * -- TB6612 is a device of the rohm. 
+ *
+ * Copyright (C) 2012 Junichi Katsu (JKSOFT) 
+ */
+
+#ifndef MBED_TB6612_H
+#define MBED_TB6612_H
+
+#include "mbed.h"
+
+class TB6612 {
+public:
+    TB6612(PinName pwm, PinName fwd, PinName rev);
+    void speed(float speed);
+    void move(float speed , float time);
+    void operator= ( float value )
+    {
+        speed(value);
+    }
+    
+protected:
+    PwmOut _pwm;
+    DigitalOut _fwd;
+    DigitalOut _rev;
+    Timeout timer;
+    float    bspeed;
+    bool     timer_flag;
+    void timeout();
+
+};
+
+#endif

diff -r 000000000000 -r a11da93ea3f6 wallbotmini.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wallbotmini.cpp	Sat Nov 02 01:04:00 2013 +0000
@@ -0,0 +1,129 @@
+/* wallbot mini Library
+ *
+ * wallbotmini.cpp
+ *
+ * Copyright (c) 2010-2013 jksoft
+ *
+ * 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:
+ *
+ * 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.
+ */
+
+#include "mbed.h"
+#include "wallbotmini.h"
+
+
+wallbotmini::wallbotmini() :  _right(dp2,dp17,dp25) , _left(dp1,dp28,dp26) , _sw(dp24),
+							  _statusled(dp13,dp6,dp14,dp18), _sensor1(dp9),_sensor2(dp10),_sensor3(dp11)  {
+	_right = 0.0;
+    _left = 0.0;
+	
+	_statusled = 0;
+}
+
+void wallbotmini::left_motor (float speed) {
+    _left = speed;
+}
+
+void wallbotmini::right_motor (float speed) {
+    _right = speed;
+}
+
+void wallbotmini::forward (float speed) {
+    _left = speed;
+	_right = speed;
+}
+
+void wallbotmini::backward (float speed) {
+    _left = -1.0*speed;
+	_right = -1.0*speed;
+}
+
+void wallbotmini::left (float speed) {
+    _left = -1.0*speed;
+	_right = speed;
+}
+
+void wallbotmini::right (float speed) {
+    _left = speed;
+	_right = -1.0*speed;
+}
+
+void wallbotmini::stop (void) {
+    _right = 0.0;
+    _left = 0.0;
+}
+
+void wallbotmini::set_led(int bit) {
+	_statusled = bit;
+}
+
+void wallbotmini::sensor_calibrate(void) {
+
+			   
+	for(int i = 0 ; i < 3 ; i++)
+	{
+		_sensor_def[i] = 0;
+	}
+
+	for(int j = 0 ; j < 12 ; j++)
+	{
+		int in[3] = { _sensor1.read_u16(),
+					 _sensor2.read_u16(),
+					 _sensor3.read_u16()
+				   };
+		set_led(1<<(j%4));
+		for(int i = 0 ; i < 3 ; i++)
+		{
+			_sensor_def[i] += in[i];
+		}
+		wait_ms(200);
+	}
+	for(int i = 0 ; i < 3 ; i++)
+	{
+		_sensor_def[i] /= 12;
+	}
+	set_led(0);
+}
+
+int wallbotmini::GetLinePosition(void) {
+	int in[3] = { _sensor1.read_u16(),
+				 _sensor2.read_u16(),
+				 _sensor3.read_u16()
+			   };
+	int stat = 0;
+	
+//	printf("ad[0]:%05d ad[1]:%05d ad[2]:%05d \n\r",in[0],in[1],in[2]);
+	
+	for(int i = 0 ; i < 3 ; i++)
+	{		
+		if( (in[i] > (_sensor_def[i] + SENSOR_VAL_WIDTH))
+		  ||(in[i] < (_sensor_def[i] - SENSOR_VAL_WIDTH)) )
+		{
+			stat |= 1 << i;
+		}
+	}
+    
+    return(stat);
+}
+
+
+int wallbotmini::GetSw(void) {
+	return(!_sw);
+}
+
+

diff -r 000000000000 -r a11da93ea3f6 wallbotmini.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wallbotmini.h	Sat Nov 02 01:04:00 2013 +0000
@@ -0,0 +1,153 @@
+/* mbed wallbot mini Library
+ *
+ * wallbotmini.h
+ *
+ * Copyright (c) 2010-2013 jksoft
+ *
+ * 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:
+ *
+ * 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.
+ */
+
+#ifndef WALLBOT_H
+#define WALLBOT_H
+
+#include "mbed.h"
+#include "TB6612.h"
+
+#define SENSOR_VAL_WIDTH	2000
+
+/** wallbot mini control class
+ *
+ * Example:
+ * @code
+ * // Drive the wwallbot forward, turn left, back, turn right, at half speed for half a second
+
+#include "mbed.h"
+#include "wallbotmini.h"
+
+wallbotmini wb;
+ 
+int main() {
+
+    wb.sensor_calibrate();
+
+    while(!wb.GetSw())
+    {
+        wb.set_led(wb.GetLinePosition());
+    }
+    
+    wb.forward(1.0);
+    wait (1.0);
+    wb.left(1.0);
+    wait (1.0);
+    wb.backward(1.0);
+    wait (1.0);
+    wb.right(1.0);
+    wait (1.0);
+    
+    wb.stop();
+
+    while(1);
+
+ }
+
+ * @endcode
+ */
+class wallbotmini  {
+
+    // Public functions
+public:
+
+    /** Create the wallbot object connected to the default pins
+     */
+    wallbotmini();
+
+    /** Sensor calibrate
+     *
+     */
+    void sensor_calibrate (void);
+
+    /** Directly control the speed and direction of the left motor
+     *
+     * @param speed A normalised number -1.0 - 1.0 represents the full range.
+     */
+    void left_motor (float speed);
+
+    /** Directly control the speed and direction of the right motor
+     *
+     * @param speed A normalised number -1.0 - 1.0 represents the full range.
+     */
+    void right_motor (float speed);
+
+    /** Drive both motors forward as the same speed
+     *
+     * @param speed A normalised number 0 - 1.0 represents the full range.
+     */
+    void forward (float speed);
+
+    /** Drive both motors backward as the same speed
+     *
+     * @param speed A normalised number 0 - 1.0 represents the full range.
+     */
+    void backward (float speed);
+
+    /** Drive left motor backwards and right motor forwards at the same speed to turn on the spot
+     *
+     * @param speed A normalised number 0 - 1.0 represents the full range.
+     */
+    void left (float speed);
+
+    /** Drive left motor forward and right motor backwards at the same speed to turn on the spot
+     * @param speed A normalised number 0 - 1.0 represents the full range.
+     */
+    void right (float speed);
+
+    /** Stop both motors
+     *
+     */
+    void stop (void);
+	
+    /** Get floorline position.(int value return.)
+     *
+     */	
+	int GetLinePosition(void);
+	
+    /** Get switch .(switch OFF:0 or ON:1 return.)
+     *
+     */	
+	int GetSw(void);
+	
+    /** Set status led .
+     * @param led (bit0:LEFT bit1:UP bit2:RIGHT bit3 DOWN)
+     */	
+	void set_led(int bit);
+	
+	private :
+
+	TB6612 _right;
+	TB6612 _left;
+	DigitalIn _sw;
+	BusOut _statusled;
+	AnalogIn _sensor1;
+	AnalogIn _sensor2;
+	AnalogIn _sensor3;
+	
+	int _sensor_def[3];    
+};
+
+#endif
\ No newline at end of file