Kevin Lee
This is a Shield Bot Library
Fork of
Shield_Bot_v12
by 
Kevin Lee
Diff: ShieldBot_v12.cpp
- Revision:
- 1:1d96ab1dbcc4
- Parent:
- 0:7535295d1670
--- 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