Diff: wallbotLED/wallbotLED.cpp

Revision:

0:9fcc79b3f00e

Child:

1:b9cd4ab5ead7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wallbotLED/wallbotLED.cpp	Fri Feb 03 06:26:54 2017 +0000
@@ -0,0 +1,358 @@
+/* JKsoft Wallbot LED Library
+ *
+ * wallbotLED.cpp
+ *
+ * Copyright (c) 2010-2016 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 "wallbotLED.h"
+
+
+wallbotLED::wallbotLED() : _right(P0_28,P0_30,P0_0) , _left(P0_29,P0_23,P0_24) ,
+                           _i2c(P0_22,P0_21), _matrix1(&_i2c),_matrix2(&_i2c),
+                           _sw(P0_16,P0_17),
+                           _outlow(P0_20),_statusled(P0_18,P0_19),
+                           _f_sensor1(P0_2),_f_sensor2(P0_3),
+                           _f_sensor3(P0_4),_f_sensor4(P0_5)  {
+	_right = 0.0;
+    _left = 0.0;
+    _outlow = 0;	
+	_statusled = 0x3;
+	SetFloorSensorGain(SENSOR_VAL_GAIN_DEFAULT);
+	_mpu.initialize();
+	_sw.mode(PullUp);
+	//_matrix1.begin(0x71);
+	_matrix1.begin(0x74);
+    _matrix2.begin(0x70);
+    _matrix1.clear();
+    _matrix2.clear();
+	for(int i = 0 ; i < 4 ; i++)
+	{		
+		_sensor_def[i] = SENSOR_VAL_DEFAULT;
+	}
+}
+
+void wallbotLED::driveLeftMotor(float speed) {
+    _left = speed;
+}
+
+void wallbotLED::driveRightMotor(float speed) {
+    _right = speed;
+}
+
+void wallbotLED::driveMotor(float r_speed,float l_speed) {
+    driveRightMotor(r_speed);
+	driveLeftMotor(l_speed);
+}
+
+void wallbotLED::driveMotor(int r_speed,int l_speed) {
+    driveRightMotor(r_speed/100.0);
+	driveLeftMotor(l_speed/100.0);
+}
+
+void wallbotLED::forward(float speed) {
+    driveLeftMotor(speed);
+	driveRightMotor(speed);
+}
+
+void wallbotLED::backward(float speed) {
+    driveLeftMotor(-speed);
+	driveRightMotor(-speed);
+}
+
+void wallbotLED::leftTurn(float speed) {
+    driveLeftMotor(-speed);
+	driveRightMotor(speed);
+}
+
+void wallbotLED::rightTurn(float speed) {
+    driveLeftMotor(speed);
+	driveRightMotor(-speed);
+}
+
+void wallbotLED::stop(void) {
+    driveLeftMotor(0.0);
+	driveRightMotor(0.0);
+}
+
+void wallbotLED::SetLed(int bit) {
+	bit |= bit >> 2;
+	_statusled = (bit ^ 0x03) & 0x03;
+}
+
+void wallbotLED::SetLed1(int bit) {
+	if( bit )
+	{
+		bit = _statusled ^ 0x03;
+		bit = bit | 0x1;
+	}
+	else
+	{
+		bit = _statusled ^ 0x03;
+		bit = bit & 0x2;
+	}
+	SetLed(bit);
+}
+
+void wallbotLED::SetLed2(int bit) {
+	if( bit )
+	{
+		bit = _statusled ^ 0x03;
+		bit = bit | 0x2;
+	}
+	else
+	{
+		bit = _statusled ^ 0x03;
+		bit = bit & 0x1;
+	}
+	SetLed(bit);
+}
+
+void wallbotLED::SetFloorSensorGain(int gain) {
+	_sensor_gain = gain;
+}
+
+void wallbotLED::FloorSensorCalibrate(void) {
+
+	for(int i = 0 ; i < 4 ; i++)
+	{
+		_sensor_def[i] = 0;
+	}
+
+	for(int j = 0 ; j < 12 ; j++)
+	{
+		int in[4] = { _f_sensor1.read_u16(),
+					 _f_sensor2.read_u16(),
+					 _f_sensor3.read_u16(),
+					 _f_sensor4.read_u16()
+				   };
+		SetLed(1<<(j%2));
+		for(int i = 0 ; i < 4 ; i++)
+		{
+			_sensor_def[i] += in[i];
+		}
+		wait_ms(200);
+	}
+	for(int i = 0 ; i < 4 ; i++)
+	{
+		_sensor_def[i] /= 12;
+	}
+	SetLed(0);
+}
+
+int wallbotLED::GetLinePosition(void) {
+	int in[4] = { _f_sensor1.read_u16(),
+				 _f_sensor2.read_u16(),
+				 _f_sensor3.read_u16(),
+				 _f_sensor4.read_u16()
+			   };
+	int stat = 0;
+	
+	// printf("ad[0]:%05d ad[1]:%05d ad[2]:%05d ad[3]:%05d \n\r",in[0],in[1],in[2],in[3]);
+	
+	for(int i = 0 ; i < 4 ; i++)
+	{		
+		if( (in[i] > (_sensor_def[i] + _sensor_gain))
+		  ||(in[i] < (_sensor_def[i] - _sensor_gain)) )
+		{
+			stat |= 1 << i;
+		}
+	}
+    
+    return(stat);
+}
+
+
+int wallbotLED::GetSw(void) {
+	return(_sw ^ 0x3);
+}
+
+bool wallbotLED::GetSw1(void)
+{
+	return(bool)(GetSw() & 0x01);	
+}
+
+bool wallbotLED::GetSw2(void)
+{
+	return(bool)(GetSw() & 0x02);	
+}
+
+bool wallbotLED::GetFloorSensor1()
+{
+	int in = _f_sensor1.read_u16();
+	
+	if( (in > (_sensor_def[0] + _sensor_gain))
+	  ||(in < (_sensor_def[0] - _sensor_gain)) )
+	{
+		return true;
+	}
+	
+	return false;
+}
+
+bool wallbotLED::GetFloorSensor2()
+{
+	int in = _f_sensor2.read_u16();
+	
+	if( (in > (_sensor_def[1] + _sensor_gain))
+	  ||(in < (_sensor_def[1] - _sensor_gain)) )
+	{
+		return true;
+	}
+	
+	return false;
+}
+
+bool wallbotLED::GetFloorSensor3()
+{
+	int in = _f_sensor3.read_u16();
+	
+	if( (in > (_sensor_def[2] + _sensor_gain))
+	  ||(in < (_sensor_def[2] - _sensor_gain)) )
+	{
+		return true;
+	}
+	
+	return false;
+}
+
+bool wallbotLED::GetFloorSensor4()
+{
+	int in = _f_sensor4.read_u16();
+	
+	if( (in > (_sensor_def[3] + _sensor_gain))
+	  ||(in < (_sensor_def[3] - _sensor_gain)) )
+	{
+		return true;
+	}
+	
+	return false;
+}
+
+uint16_t wallbotLED::GetFloorSensorRaw1()
+{
+	return _f_sensor1.read_u16();
+}
+
+uint16_t wallbotLED::GetFloorSensorRaw2()
+{
+	return _f_sensor2.read_u16();
+}
+
+uint16_t wallbotLED::GetFloorSensorRaw3()
+{
+	return _f_sensor3.read_u16();
+}
+
+uint16_t wallbotLED::GetFloorSensorRaw4()
+{
+	return _f_sensor4.read_u16();
+}
+
+void wallbotLED::GetAcceleration(float *x,float *y,float *z)
+{
+	int16_t _x,_y,_z;
+	_mpu.getAcceleration(&_x,&_y,&_z);
+
+	*x = (float)_x / 8192;
+	*y = (float)_y / 8192;
+	*z = (float)_z / 8192;
+}
+
+float wallbotLED::GetAccelerationX()
+{	
+	return (float)_mpu.getAccelerationX() / 8192;
+}
+
+float wallbotLED::GetAccelerationY()
+{
+	return (float)_mpu.getAccelerationY() / 8192;
+}
+
+float wallbotLED::GetAccelerationZ()
+{
+	return (float)_mpu.getAccelerationZ() / 8192;
+}
+
+void wallbotLED::GetGyro(float *x,float *y,float *z)
+{
+	int16_t _x,_y,_z;
+	_mpu.getRotation(&_x,&_y,&_z);
+
+	*x = (float)_x / 131;
+	*y = (float)_y / 131;
+	*z = (float)_z / 131;
+}
+
+float wallbotLED::GetGyroX()
+{
+	return (float)_mpu.getAccelerationX() / 131;
+}
+
+float wallbotLED::GetGyroY()
+{
+	return (float)_mpu.getAccelerationY() / 131;
+}
+
+float wallbotLED::GetGyroZ()
+{
+	return (float)_mpu.getAccelerationZ() / 131;
+}
+
+void wallbotLED::SetLedPanel(char data[][16])
+{
+    for (int y = 0; y < 8; y++) {
+      for (int x = 0; x < 16; x++) {
+        if( data[x][y+8] == 1 )
+        {
+            _matrix1.drawPixel(x, y, LED_ON);
+        }
+        else
+        {
+            _matrix1.drawPixel(x, y, LED_OFF);
+        }
+        if( data[x][y] == 1 )
+        {
+            _matrix2.drawPixel(x, y, LED_ON);
+        }
+        else
+        {
+            _matrix2.drawPixel(x, y, LED_OFF);
+        }
+      }
+    }
+    _matrix1.writeDisplay();
+    _matrix2.writeDisplay();
+}
+
+void wallbotLED::SetLedPanel(int x, int y, int flag)
+{
+	if(y < 8) {
+		_matrix1.drawPixel(x, y, flag);
+        _matrix1.writeDisplay();
+	}
+	else {
+		_matrix2.drawPixel(x, y-8, flag);
+        _matrix2.writeDisplay();
+	}
+}
+