File content as of revision 1:b9cd4ab5ead7:

/* 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();
	}
}