This is a Shield Bot Library
Fork of Shield_Bot_v12 by
Revision 1:1d96ab1dbcc4, committed 2015-09-15
- Comitter:
- Kevin_Lee
- Date:
- Tue Sep 15 13:40:55 2015 +0000
- Parent:
- 0:7535295d1670
- Commit message:
- This is a library for ShieldBot
Changed in this revision
ShieldBot_v12.cpp | Show annotated file Show diff for this revision Revisions of this file |
ShieldBot_v12.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/ShieldBot_v12.cpp Wed Sep 09 05:53:29 2015 +0000 +++ b/ShieldBot_v12.cpp Tue Sep 15 13:40:55 2015 +0000 @@ -1,23 +1,24 @@ -/** Color pixels library using WS2812B and nRF51822 (16MHz) - * It's for - * + http://www.seeedstudio.com/depot/Digital-RGB-LED-FlexiStrip-60-LED-1-Meter-p-1666.html - * + http://www.seeedstudio.com/depot/WS2812B-Digital-RGB-LED-Waterproof-FlexiStrip-144-LEDmeter-2-meter-p-1869.html - * + http://www.seeedstudio.com/depot/WS2812B-RGB-LED-with-Integrated-Driver-Chip-10-PCs-pack-p-1675.html +/* + * ShieldBot_v12.cpp + * A library for ShieldBot + * + * Copyright (c) 2015 seeed technology inc. + * Author : Jiankai.li + * Create Time: Sep 2015 + * Change Log : * * The MIT License (MIT) * - * Copyright (c) 2014 Seeed Technology Inc. - * * 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 @@ -27,274 +28,220 @@ * THE SOFTWARE. */ +#include "mbed.h" #include "ShieldBot_v12.h" #include "nrf51.h" #include "nrf51_bitfields.h" +/* + Created to support the release of the Sheildbot from SeeedStudios + http://www.seeedstudio.com/wiki/Shield_Bot -#if !defined ( __GNUC__ ) -// Generate a high level pulse (0.81us) of WS2812B's 1 code (0.9us +- 0.15us) -#define COLOR_PIXELS_ONE_HIGH(mask) \ - NRF_GPIO->OUTSET = (mask); \ - __ASM ( \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - ); \ - NRF_GPIO->OUTCLR = (mask) + Created by Jacob Rosenthal and Colin Ho, December 30, 2012. + Released into the public domain. +*/ + + +#define SHIELDBOTDEBUG 0 + +#define right1 p18 //define I1 interface +#define speedPinRight p23 //enable right motor (bridge A) +#define right2 p25 //define I2 interface +#define left1 p28 //define I3 interface +#define speedPinLeft p24 //enable motor B +#define left2 p29 //define I4 interface + +#define HIGH 1 +#define LOW 0 -// Generate a high level pulse (0.31us) of WS2812B's 0 code (0.35us +- 0.15us) -#define COLOR_PIXELS_ZERO_HIGH(mask) \ - NRF_GPIO->OUTSET = (mask); \ - __ASM ( \ - " NOP\n\t" \ - ); \ - NRF_GPIO->OUTCLR = (mask); \ - __ASM ( \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - ) -#else -// Generate a high level pulse (0.94us) of WS2812B's 1 code (0.9us +- 0.15us) -#define COLOR_PIXELS_ONE_HIGH(mask) \ - NRF_GPIO->OUTSET = (mask); \ - __ASM ( \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - ); \ - NRF_GPIO->OUTCLR = (mask) +PwmOut MotorRight(speedPinRight); +DigitalIn finder1(p1); // define line finder S1 +DigitalIn finder2(p2); // define line finder S2 +DigitalIn finder3(p3); // define line finder S3 +DigitalIn finder4(p4); // define line finder S4 +DigitalIn finder5(p11); // define line finder S5 -// Generate a high level pulse (0.44us) of WS2812B's 0 code (0.35us +- 0.15us) -#define COLOR_PIXELS_ZERO_HIGH(mask) \ - NRF_GPIO->OUTSET = (mask); \ - __ASM ( \ - " NOP\n\t" \ - ); \ - NRF_GPIO->OUTCLR = (mask); \ - __ASM ( \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - " NOP\n\t" \ - ) -#endif +DigitalOut Right1(right1); +DigitalOut Right2(right2); +DigitalOut Left1(left1); +DigitalOut Left2(left2); + +PwmOut SpeedRight(speedPinRight); +PwmOut SpeedLeft(speedPinLeft); -static void delay_us(uint32_t us) +int speedmotorA = 255; //define the speed of motorA +int speedmotorB = 255; //define the speed of motorB + +Shieldbot::Shieldbot() { - do { - __ASM volatile ( - "NOP\n\t" - "NOP\n\t" - "NOP\n\t" - "NOP\n\t" - "NOP\n\t" - "NOP\n\t" - "NOP\n\t" - "NOP\n\t" - "NOP\n\t" - "NOP\n\t" - "NOP\n\t" - "NOP\n\t" - ); - } while (--us); + SpeedRight.period_us(100); + SpeedLeft.period_us(100); } -ColorPixels::ColorPixels(uint8_t pin, uint16_t num) -{ - this->pin = pin; - this->num = num; +//sets same max speed to both motors +void Shieldbot::setMaxSpeed(int both){ + setMaxLeftSpeed(both); + setMaxRightSpeed(both); +} - NRF_GPIO->PIN_CNF[pin] = (GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos) - | (GPIO_PIN_CNF_DRIVE_S0S1 << GPIO_PIN_CNF_DRIVE_Pos) - | (GPIO_PIN_CNF_PULL_Disabled << GPIO_PIN_CNF_PULL_Pos) - | (GPIO_PIN_CNF_INPUT_Connect << GPIO_PIN_CNF_INPUT_Pos) - | (GPIO_PIN_CNF_DIR_Output << GPIO_PIN_CNF_DIR_Pos); - NRF_GPIO->OUTCLR = (1UL << pin); +void Shieldbot::setMaxSpeed(int left, int right){ + setMaxLeftSpeed(left); + setMaxRightSpeed(right); +} - colors = new grb_t[num]; - for (int i = 0; i < num; i++) { - colors[i].grb = 0; - } +void Shieldbot::setMaxLeftSpeed(int left){ + speedmotorB=left; +} + +void Shieldbot::setMaxRightSpeed(int right){ + speedmotorA=right; } -ColorPixels::~ColorPixels() -{ - delete colors; +int Shieldbot::readS1(){ + return finder1.read(); +} + +int Shieldbot::readS2(){ + return finder2.read(); +} + +int Shieldbot::readS3(){ + return finder3.read(); } - -void ColorPixels::set_color(uint16_t index, uint8_t r, uint8_t g, uint8_t b) -{ - if (index < num) { - colors[index].r = r; - colors[index].g = g; - colors[index].b = b; - } +int Shieldbot::readS4(){ + return finder4.read(); } -void ColorPixels::set_color(uint16_t index, uint32_t rgb) { - color_t c = *(color_t *)&rgb; - set_color(index, c.r, c.g, c.b); +int Shieldbot::readS5(){ + return finder5.read(); +} + +void Shieldbot::drive(signed char left,signed char right){ + rightMotor(right); + leftMotor(left); } -void ColorPixels::set_all_color(uint8_t r, uint8_t g, uint8_t b) -{ - for (int i = 0; i < num; i++) { - colors[i].r = r; - colors[i].g = g; - colors[i].b = b; - } - - update(); -} - -void ColorPixels::update() -{ - uint32_t mask = 1 << pin; - NRF_GPIO->OUTCLR = mask; - delay_us(50); - - grb_t *pcolor = colors; - for (int i = 0; i < num; i++) { - uint32_t grb = (*pcolor).grb; - for (int bit = 0; bit < 24; bit++) { - if (grb & 1) { - COLOR_PIXELS_ONE_HIGH(mask); - } else { - COLOR_PIXELS_ZERO_HIGH(mask); - } - grb >>= 1; - } - pcolor++; - } -} - -void ColorPixels::clear() -{ - for (int i = 0; i < num; i++) { - colors[i].grb = 0; - } - - update(); +//char is 128 to 127 +//mag is the direction to spin the right motor +//-128 backwards all the way +//0 dont move +//127 forwards all the way +void Shieldbot::rightMotor(signed char mag){ + int actualSpeed = 0; + if(mag >0){ //forward + float ratio = (float)mag/128; + actualSpeed = (int)(ratio*speedmotorA); //define your speed based on global speed + #if SHIELDBOTDEBUG + Serial.print("forward right: "); + Serial.println(actualSpeed); + #endif + SpeedRight.write((float)(actualSpeed/255.0)); + // analogWrite(speedPinRight,actualSpeed); + Right1 = HIGH; + Right2 = LOW; + // digitalWrite(right1,HIGH); + // digitalWrite(right2,LOW);//turn right motor clockwise + }else if(mag == 0){ //neutral + #if SHIELDBOTDEBUG + Serial.println("nuetral right"); + #endif + stopRight(); + }else { //meaning backwards + float ratio = (float)abs(mag)/128; + actualSpeed = ratio*speedmotorA; + #if SHIELDBOTDEBUG + Serial.print("backward right: "); + Serial.println(actualSpeed); + #endif + SpeedRight.write((float)(actualSpeed/255.0)); + //analogWrite(speedPinRight,actualSpeed); + Right1 = LOW; + Right2 = HIGH; + // digitalWrite(right1,LOW); + // digitalWrite(right2,HIGH);//turn right motor counterclockwise + } } -uint32_t toRGB(uint8_t red, uint8_t green, uint8_t blue) -{ - color_t c; - - c.r = red; - c.g = green; - c.b = blue; - - return c.rgb; -}; - -uint32_t wheel(float position, uint8_t min = 0, uint8_t max = 255) -{ - uint8_t d = max - min; - uint32_t c; +//TODO shouldnt these share one function and just input the differences? +void Shieldbot::leftMotor(signed char mag){ + int actualSpeed = 0; + if(mag >0){ //forward + float ratio = (float)(mag)/128; + actualSpeed = (int)(ratio*speedmotorB); //define your speed based on global speed + #if SHIELDBOTDEBUG + Serial.print("forward left: "); + Serial.println(actualSpeed); + #endif + SpeedLeft.write((float)(actualSpeed/255.0)); + Left1 = HIGH; + Left2 = LOW; + // analogWrite(speedPinLeft,actualSpeed); + // digitalWrite(left1,HIGH); + // digitalWrite(left2,LOW);//turn left motor counter-clockwise + }else if(mag == 0){ //neutral + #if SHIELDBOTDEBUG + Serial.println("nuetral left"); + #endif + stopLeft(); + }else { //meaning backwards + float ratio = (float)abs(mag)/128; + actualSpeed = ratio*speedmotorB; + #if SHIELDBOTDEBUG + Serial.print("backward left: "); + Serial.println(actualSpeed); + #endif + SpeedLeft.write((float)(actualSpeed/255.0)); + Left1 = LOW; + Left2 = HIGH; + // analogWrite(speedPinLeft,actualSpeed); + // digitalWrite(left1,LOW); + // digitalWrite(left2,HIGH);//turn left motor counterclockwise + } +} - if(position < 0.166) { - c = toRGB(max, min + position * 6 * d, min); - } else if(position < 0.332) { - position -= 0.166; - c = toRGB(max - position * 6 * d, max, min); - } else if(position < 0.498) { - position -= 0.332; - c = toRGB(min, max, min + position * 6 * d); - } else if(position < 0.664) { - position -= 0.498; - c = toRGB(min, max - position * 6 * d, max); - } else if(position < 0.83) { - position -= 0.664; - c = toRGB(min + d * position * 6, min, max); - } else { - position -= 0.83; - c = toRGB(max, min, max - position * 6 * d); - } - - return c; +void Shieldbot::forward(){ + leftMotor(127); + rightMotor(127); } -void find_wheel_options(uint32_t rgb, float *position, uint8_t *min, uint8_t *max) -{ - color_t c; - c.rgb = rgb; - - if (c.r > c.g) { - if (c.g >= c.b) { - *max = c.r; - *min = c.b; - *position = (float)(c.g - c.b) / (c.r - c.b) / 6.0; - } else if (c.b >= c.r) { - *max = c.b; - *min = c.g; - *position = (float)(c.r - c.g) / (c.b - c.g) / 6.0 + 0.664; - } else { - *max = c.r; - *min = c.g; - *position = (float)(c.b - c.g) / (c.r - c.g) / 6.0 + 0.83; - } - } else { - if (c.r > c.b) { - *max = c.g; - *min = c.b; - *position = (float)(c.r - c.b) / (c.g - c.b) / 6.0 + 0.166; - } else if (c.b > c.g) { - *max = c.b; - *min = c.r; - *position = (float)(c.g - c.r) / (c.b - c.r) / 6.0 + 0.498; - } else { - *max = c.g; - *min = c.r; - *position = (float)(c.b - c.r) / (c.g - c.r) / 6.0 + 0.332; - } - } +void Shieldbot::backward(){ + leftMotor(-128); + rightMotor(-128); +} + +void Shieldbot::stop(){ + stopLeft(); + stopRight(); +} + +void Shieldbot::stopLeft(){ + // analogWrite(speedPinLeft,0); + SpeedLeft.write(0); +} + +void Shieldbot::stopRight(){ + //analogWrite(speedPinRight,0); + SpeedRight.write(0); } -void ColorPixels::rainbow(uint32_t rgb) -{ - float position; - uint8_t min, max; - - find_wheel_options(rgb, &position, &min, &max); - for (int i = 0; i < num; i++) { - float current = ((int)(i + num * position) % num) / (float)num; - set_color(i, wheel(current, min, max)); - } - - update(); +//may be dangerous to motor if reverse current into hbridge exceeds 2A +void Shieldbot::fastStopLeft(){ + Left1 = LOW; + Left2 = LOW; + // digitalWrite(left1,LOW); + // digitalWrite(left2,LOW);//turn DC Motor B move clockwise } -void ColorPixels::rainbow(uint8_t r, uint8_t g, uint8_t b) -{ - color_t c; - - c.r = r; - c.g = g; - c.b = b; - - rainbow(c.rgb); +//may be dangerous to motor if reverse current into hbridge exceeds 2A +void Shieldbot::fastStopRight(){ + Right1 = LOW; + Right2 = LOW; + // digitalWrite(right1,LOW); + // digitalWrite(right2,LOW);//turn DC Motor A move anticlockwise } +//may be dangerous to motor if reverse current into hbridge exceeds 2A +void Shieldbot::fastStop(){ + fastStopRight(); + fastStopLeft(); +} \ No newline at end of file
--- a/ShieldBot_v12.h Wed Sep 09 05:53:29 2015 +0000 +++ b/ShieldBot_v12.h Tue Sep 15 13:40:55 2015 +0000 @@ -1,23 +1,24 @@ -/** Color pixels library using WS2812B and nRF51822 (16Hz) - * It's for - * + http://www.seeedstudio.com/depot/Digital-RGB-LED-FlexiStrip-60-LED-1-Meter-p-1666.html - * + http://www.seeedstudio.com/depot/WS2812B-Digital-RGB-LED-Waterproof-FlexiStrip-144-LEDmeter-2-meter-p-1869.html - * + http://www.seeedstudio.com/depot/WS2812B-RGB-LED-with-Integrated-Driver-Chip-10-PCs-pack-p-1675.html +/* + * ShieldBot_v12.h + * A library for ShieldBot + * + * Copyright (c) 2015 seeed technology inc. + * Author : Jiankai.li + * Create Time: Sep 2015 + * Change Log : * * The MIT License (MIT) * - * Copyright (c) 2014 Seeed Technology Inc. - * * 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 @@ -27,19 +28,14 @@ * THE SOFTWARE. */ + +// ensure this library description is only included once + #ifndef __SHIELDBOT_V12_H__ #define __SHIELDBOT_V12_H__ #include <stdint.h> -typedef union { - uint32_t rgb; - struct { - uint8_t r; - uint8_t g; - uint8_t b; - }; -} color_t; /** Color pixels class using WS2812B and nRF51822 (16Hz) * @@ -59,53 +55,34 @@ * } * @endcode */ -class ColorPixels + +// library interface description +class Shieldbot { -public: - /** Initilaze - * \param pin number of GPIO - * \param num number of pixels - */ - ColorPixels(uint8_t pin, uint16_t num); - - ~ColorPixels(); - - /** Set the color of a pixel (without update) - * \param index index of a pixel - * \param r red - * \param g green - * \param b blue - */ - void set_color(uint16_t index, uint8_t r, uint8_t g, uint8_t b); - - void set_color(uint16_t index, uint32_t rgb); - - void set_all_color(uint8_t r, uint8_t g, uint8_t b); - - void rainbow(uint8_t r, uint8_t g, uint8_t b); - - void rainbow(uint32_t rgb = 0x0000FF); - - /** Update - */ - void update(); - - /** Turn off all pixels - */ - void clear(); - -private: - typedef union { - struct { - uint8_t g, r, b; - }; - uint32_t grb; - } grb_t; - - uint8_t pin; - uint16_t num; - grb_t *colors; + // user-accessible "public" interface + public: + Shieldbot(); + int readS1(); + int readS2(); + int readS3(); + int readS4(); + int readS5(); + void setMaxSpeed(int); + void setMaxSpeed(int, int); + void setMaxLeftSpeed(int); + void setMaxRightSpeed(int); + void rightMotor(signed char); + void leftMotor(signed char); + void drive(signed char, signed char); + void forward(); + void backward(); + void stop(); + void stopRight(); + void stopLeft(); + void fastStopLeft(); + void fastStopRight(); + void fastStop(); }; -#endif // __COLOR_PIXELS_H__ +#endif