Skip to content

Commit 68ec75d

Browse files
ROBOTIS-WillROBOTIS-Will
authored
Merge pull request #329 from ROBOTIS-GIT/hotfix-cs-10-lib-update
[1.5.3] update CS-10 library for OpenCR
2 parents b913ecf + d00f6e2 commit 68ec75d

File tree

3 files changed

+183
-101
lines changed

3 files changed

+183
-101
lines changed

‎arduino/opencr_arduino/opencr/libraries/OLLO/OLLO.cpp

+95-101
Original file line numberDiff line numberDiff line change
@@ -390,7 +390,7 @@ int OLLO::read(int devNum, OlloDeviceIndex device_index, ColorIndex sub_index){
390390
digitalWrite(PORT1_SIG1, mMot_minus);
391391
digitalWrite(PORT1_SIG2, mMot_plus);
392392
delay(5); // after 20ms, read analog
393-
return (((int)analogRead(PORT1_ADC))/4);
393+
return (((int)analogRead(PORT1_ADC)));
394394

395395
case 2:
396396
digitalWrite(PORT2_SIG1, mMot_minus);
@@ -402,13 +402,13 @@ int OLLO::read(int devNum, OlloDeviceIndex device_index, ColorIndex sub_index){
402402
digitalWrite(PORT3_SIG1, mMot_minus);
403403
digitalWrite(PORT3_SIG2, mMot_plus);
404404
delay(5);
405-
return ((int)analogRead(PORT3_ADC)/4);
405+
return ((int)analogRead(PORT3_ADC));
406406

407407
case 4:
408408
digitalWrite(PORT4_SIG1, mMot_minus);
409409
digitalWrite(PORT4_SIG2, mMot_plus);
410410
delay(5);
411-
return ((int)analogRead(PORT4_ADC)/4);
411+
return ((int)analogRead(PORT4_ADC));
412412

413413
default:
414414
return 0;
@@ -501,120 +501,114 @@ void OLLO::write(int devNum, uint8_t leftVal, uint8_t centerVal, uint8_t rightVa
501501

502502
}
503503

504-
void OLLO::setColor(ColorIndex colorIndex){
504+
void OLLO::setColor(ColorIndex colorIndex) {
505505
switch(colorIndex){
506-
case RED: //Red
507-
mMot_minus = LOW;
508-
mMot_plus = LOW;
509-
break;
510-
case GREEN://Green
511-
mMot_minus = LOW;
512-
mMot_plus = HIGH;
513-
break;
514-
case BLUE://Blue
515-
mMot_minus = HIGH;
516-
mMot_plus = LOW;
517-
break;
518-
default:
519-
break;
520-
}
521-
506+
case RED: //Red
507+
mMot_minus = HIGH;
508+
mMot_plus = HIGH;
509+
break;
510+
case GREEN://Green
511+
mMot_minus = LOW;
512+
mMot_plus = HIGH;
513+
break;
514+
case BLUE://Blue
515+
mMot_minus = HIGH;
516+
mMot_plus = LOW;
517+
break;
518+
default:
519+
break;
520+
}
522521
}
523522

524-
525-
int OLLO::detectColor(uint8_t port){
526-
527-
// int temp_red,temp_green,temp_blue;
528-
529-
// temp_red = 0;
530-
// temp_green = 0;
531-
// temp_blue= 0;
523+
int OLLO::detectColor(uint8_t port) {
532524
int lColor[3]= {0,0,0};
533525
int lRed,lGreen,lBlue;
534-
//int bMaxColor, bMinColor;
535-
//bMaxColor=0;
536-
//bMinColor=0;
537-
int bColorResult;
538526
bColorResult=0;
539527

540-
lRed = this->read(port, COLOR_SENSOR, RED);
541-
//for(i=0; i < 3; i++)
528+
float fRawY, fRawCb, fRawCr, fR, fG, fB, fGR, fGB, fRB;
529+
uint8_t uY, uCb, uCr;
542530

531+
lRed = this->read(port, COLOR_SENSOR, RED);
543532
lGreen = (this->read(port, COLOR_SENSOR, GREEN));
544-
//for(i=0; i < 3; i++)
545-
546533
lBlue = this->read(port, COLOR_SENSOR, BLUE);
547-
548-
if(lRed >= lGreen && lRed >= lBlue)
549-
{
550-
//bMaxColor = 1;
551-
lColor[0] = lRed;
552-
}
553-
else if(lGreen >= lRed && lGreen >= lBlue)
554-
{
555-
//bMaxColor = 2;
556-
lColor[0] = lGreen;
557-
}
558-
559-
else if(lBlue >= lRed && lBlue >= lGreen)
560-
{
561-
//bMaxColor = 3;
562-
lColor[0] = lBlue;
563-
}
564-
if(lRed <= lGreen && lRed <= lBlue)
565-
{
566-
//bMinColor = 1;
567-
lColor[2] = lRed;
568-
}
569-
else if(lGreen <= lRed && lGreen <= lBlue)
570-
{
571-
//bMinColor = 2;
572-
lColor[2] = lGreen;
573-
}
574-
575-
else if(lBlue <= lRed && lBlue <= lGreen)
534+
535+
fGR = (float)lGreen / (float)lRed;
536+
fGB = (float)lGreen / (float)lBlue;
537+
fRB = (float)lRed / (float)lBlue;
538+
fR = (float)lRed * 255 / 1023;
539+
fG = (float)lGreen * 255 / 1023;
540+
fB = (float)lBlue * 255 / 1023;
541+
542+
fRawY = (299 * fR + 587 * fG + 114 * fB) / 1000;
543+
fRawCb = 0.5643 * (fB - fRawY) + 128;
544+
fRawCr = 0.7132 * (fR - fRawY) + 128;
545+
546+
uY = constrain((int)fRawY , 0 , 255);
547+
uCb = constrain((int)fRawCb, 0 , 255);
548+
uCr = constrain((int)fRawCr, 0 , 255);
549+
550+
// Color detecting algorithm changed from RGB to YCbCr, 2018-10-18 Will
551+
if (uCb >= 105 && uCb <= 125 && uCr >= 130 && uCr < 200 && uY <= 100)
576552
{
577-
//bMinColor = 3;
578-
lColor[2] = lBlue;
553+
if ((fGR < 0.60) && ((fRB > 2.5) || (fRB <= 2.5 && fGB < 1.15))) {
554+
bColorResult = 3; // red
555+
} else if ((uCb / uY) < 1.50) {
556+
bColorResult = 6; // yellow
557+
} else {
558+
bColorResult = 0; // unknown
559+
}
560+
} else if (uCb >= 110 && uCb <= 135 && uCr >= 105 && uCr < 140 && uY > 40 && uY < 100) {
561+
if (fGR > 0.90) {
562+
if (fGB > 0.80) {
563+
bColorResult = 4; // green
564+
} else {
565+
bColorResult = 0;
566+
}
567+
} else {
568+
bColorResult = 6; // yellow
569+
}
570+
} else if (uCb >= 130 && uCb <= 160 && uCr >= 110 && uCr < 130 && uY > 20 && uY < 80) {
571+
if (fGR > 1.00 && fGB < 0.80) {
572+
bColorResult = 5; // blue
573+
} else if (fRB > 0.90) {
574+
bColorResult = 2; // black
575+
} else {
576+
bColorResult = 0;
577+
}
578+
} else if (uCb >= 70 && uCb <= 115 && uCr >= 130 && uCr < 185 && uY > 80) {
579+
if ((float)(uCb / uY) < 1.50) {
580+
bColorResult = 6; // yellow
581+
} else if ((float)(uCb / uY) > 2.00) {
582+
bColorResult = 2; // black
583+
} else {
584+
bColorResult = 0; // unknown
585+
}
586+
} else if (uCb >= 110 && uCb <= 130 && uCr >= 110 && uCr < 140 && uY < 100) {
587+
if (fGR < 1.10 || (fGR >=1.10 && lRed < 90 && lGreen < 100 && lBlue < 90)) {
588+
bColorResult = 2; // black
589+
} else if (lGreen >= 100) {
590+
bColorResult = 4; // Green
591+
} else {
592+
bColorResult = 0; // unknown
593+
}
594+
} else if (uCb >= 105 && uCb <= 150 && uCr >= 110 && uCr < 165 && uY > 100) {
595+
if (fRB < 1.30) {
596+
bColorResult = 1; // white
597+
} else if (fRB > 1.30) {
598+
bColorResult = 6; // yellow
599+
} else {
600+
bColorResult = 0; // unknown
601+
}
602+
} else {
603+
bColorResult = 0; // unknown
579604
}
580605

581-
lColor[1] = lRed + lGreen + lBlue - lColor[0] - lColor[2];
582-
583-
uint32_t RtoB = lRed * 100 / lBlue;
584-
uint32_t GtoB = lGreen * 100 / lBlue;
585-
uint32_t GtoR = lGreen * 100 / lRed;
586-
587-
//2014-03-24 [email protected]
588-
if(lColor[0] < 90 || ( lColor[0] < 180 &&
589-
RtoB > 50 &&
590-
(GtoB < 110 || GtoR < 130) &&
591-
(GtoB + GtoR < 230) &&
592-
((lColor[2] * 100 / lColor[0]) > 75)
593-
)
594-
){//end of if()
595-
bColorResult = 2; // blackz
596-
}
597-
else if((lColor[2] > 550) || ((lColor[2] > 200) && (lColor[0] > 300) && (lColor[2] * 100 / lColor[0] > 75) && (GtoB < 105)))
598-
bColorResult = 1; // white
599-
else if(RtoB > 170 && GtoB > 130)
600-
bColorResult = 6; // yellow
601-
else if(RtoB > 170 && GtoB <= 130)
602-
bColorResult = 3; // red
603-
else if(GtoB > 80 && GtoR >= 100)//90 110
604-
bColorResult = 4; // green
605-
else if(RtoB < 70 && GtoB <= 85)
606-
bColorResult = 5; // blue
607-
else
608-
bColorResult = 0; // unknown
609-
610-
if(bColorResult == before_color_num){
606+
if (bColorResult == before_color_num) {
611607
before_color_cnt++;
612-
if(before_color_cnt >= 10){
613-
//before_color_cnt = 0;
608+
if (before_color_cnt >= 10) {
614609
return bColorResult;
615610
}
616-
}
617-
else{
611+
} else {
618612
before_color_cnt = 0;
619613
}
620614

‎arduino/opencr_arduino/opencr/libraries/OpenCR/examples/05. Sensors/OLLO_COLOR_Read/OLLO_COLOR_Read.ino

+57
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
// OLLO Color Sensor(CS-10) example
2+
//
3+
// Connect Color Sensor Module(CS-10) to OLLO port 1 in the OpenCR.
4+
//
5+
// You can buy Color Sensor in ROBOTIS-SHOP
6+
// http://www.robotis-shop-en.com/shop/step1.php?number=750&b_code=B20070914051413&c_code=C20100528062452
7+
// You can also find all information
8+
// http://support.robotis.com/
9+
//
10+
// This example is tested with the following device
11+
// Controller : OpenCR
12+
// Sensor : CS-10
13+
//
14+
// Created : 13 July 2023
15+
// by YKW. ROBOTIS Co., LTD.
16+
17+
#include <OLLO.h>
18+
OLLO myOLLO;
19+
20+
void setup() {
21+
myOLLO.begin(1, COLOR_SENSOR); // OLLO Color Module must be connected at port 1.
22+
Serial.begin(115200);
23+
}
24+
25+
void loop() {
26+
int colorValue = myOLLO.read(1, COLOR_SENSOR);
27+
28+
Serial.print("COLOR Sensor Read = ");
29+
switch (colorValue) {
30+
case 0:
31+
Serial.println("Unknown color");
32+
break;
33+
case 1:
34+
Serial.println("White");
35+
break;
36+
case 2:
37+
Serial.println("Black");
38+
break;
39+
case 3:
40+
Serial.println("Red");
41+
break;
42+
case 4:
43+
Serial.println("Green");
44+
break;
45+
case 5:
46+
Serial.println("Blue");
47+
break;
48+
case 6:
49+
Serial.println("Yellow");
50+
break;
51+
default:
52+
Serial.println("Unknown color");
53+
break;
54+
}
55+
56+
delay(100);
57+
}

‎arduino/opencr_release/package_opencr_index.json

+31
Original file line numberDiff line numberDiff line change
@@ -1527,6 +1527,37 @@
15271527
"version": "1.0.0"
15281528
}
15291529
]
1530+
},
1531+
{
1532+
"name": "OpenCR",
1533+
"architecture": "OpenCR",
1534+
"version": "1.5.3",
1535+
"category": "Arduino",
1536+
"help": {
1537+
"online": "https://github.com/ROBOTIS-GIT/OpenCR"
1538+
},
1539+
"url": "https://github.com/ROBOTIS-GIT/OpenCR/releases/download/1.5.3/opencr.tar.bz2",
1540+
"archiveFileName": "opencr.tar.bz2",
1541+
"checksum": "SHA-256:418656e5e6d99d45d187ffdb28dece0f450c6707da3f6db56769f3ecafdc413c",
1542+
"size": "2690860",
1543+
"help": {
1544+
"online": "http://emanual.robotis.com/docs/en/parts/controller/opencr10/"
1545+
},
1546+
"boards": [
1547+
{"name": "OpenCR"}
1548+
],
1549+
"toolsDependencies": [
1550+
{
1551+
"packager": "OpenCR",
1552+
"name": "opencr_gcc",
1553+
"version": "5.4.0-2016q2"
1554+
},
1555+
{
1556+
"packager": "OpenCR",
1557+
"name": "opencr_tools",
1558+
"version": "1.0.0"
1559+
}
1560+
]
15301561
}
15311562
],
15321563
"tools":[

0 commit comments

Comments
 (0)