Diff: wallbotmini.cpp

Revision:

0:a11da93ea3f6

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);
+}
+
+