【求教】舵机和颜色传感器连接后 不工作

代码如下：
#include <TimerOne.h> // 引用 TimerOne.h 库文件
#include <Servo.h> // 引用Servo.h 库文件
#define S0 6 // 定义 S0为引脚6
#define S1 5 // 定义 S1为引脚5
#define S2 4 // 定义 S2为引脚4
#define S3 3 // 定义 S3为引脚3
#define OUT 2 // 定义 OUT为引脚2
int g_count = 0; //定义整型变量 g_count并赋初值为0，用于存储计数频率
int g_array[3]; //定义整型数组变量 g_array[3]，用于存储RGB的值
int g_flag = 0; //定义整形变量 g_flag 并赋初值为0，用于过滤器排列
float g_SF[3]; //定义浮点型数组变量g_SF[3]，用于存储RGB比例因子
int value[3]; //定义定义整型数组变量value[3]，用于判断颜色
// 初始化 tsc3200 和设置内置振荡器方波频率与其输出信号频率的比例因子为2%
Servo myservo; //创建一个舵机控制对象
int pos = 0; // 该变量用与存储舵机角度位置
void TSC_Init()
{
pinMode(S0, OUTPUT); // 定义S0为输出状态
pinMode(S1, OUTPUT); // 定义S1为输出状态
pinMode(S2, OUTPUT); // 定义S2为输出状态
pinMode(S3, OUTPUT); // 定义S3为输出状态
pinMode(OUT, INPUT); // 定义OUT为输入状态
digitalWrite(S0, LOW); //定义S0为低电平
digitalWrite(S1, HIGH); // 定义 S1为高电平
//输出频率缩放 2%
}
// 选择滤波模式，决定让红、绿、蓝哪一种光线通过滤波器
void TSC_FilterColor(int Level01, int Level02)
{
if(Level01 != 0) // 如果Level01 不等于0
Level01 = HIGH; //则Level01 为高电平
if(Level02 != 0) // 如果Level02 不等于0
Level02 = HIGH; //则Level02 为高电平
digitalWrite(S2, Level01); // 将Level01值送给S2
digitalWrite(S3, Level02); // 将Level02值送给S3
// 选择过滤器颜色
}
//中断函数，计算 TCS3200 输出信号的脉冲数
void TSC_Count()
{
g_count ++ ;
}
/*定时器中断函数，每 1s 中断后，把该时间内的红、绿、蓝三种广信通过滤波器时，
* TCS3200 输出信号脉冲个数分别储存到数组 g_array[3] 的相应元素变量中
*/
void TSC_Callback()
{
switch(g_flag)
{
case 0:
Serial.println("->WB Start");// 串口打印字符串"->WB Start"
TSC_WB(LOW, HIGH);// 选择让蓝色光线通过滤波器的模式
break;
case 1:
Serial.print("->Frequency B="); // 串口打印字符串"->Frequency B="
Serial.println(g_count);// 串口打印 1s 内的蓝光通过滤波器时，TCS3200 输出的脉冲个数
g_array[0] = g_count;//储存 1s 内的蓝光通过滤波器时，TCS3200 输出的脉冲个数
TSC_WB(HIGH, HIGH);// 选择让绿色光线通过滤波器的模式
break;
/*case 2:
Serial.print("->Frequency G="); // 串口打印字符串"->Frequency G="
Serial.println(g_count);// 串口打印 1s 内的绿光通过滤波器时，TCS3200 输出的脉冲个数
g_array[1] = g_count;//储存 1s 内的绿光通过滤波器时，TCS3200 输出的脉冲个数
TSC_WB(LOW, LOW);//选择让红色光线通过滤波器的模式
break;
case 3:
Serial.print("->Frequency R="); // 串口打印字符串"->Frequency R="
Serial.println(g_count);// 串口打印 1s 内的蓝光通过滤波器时，TCS3200 输出的脉冲个数
Serial.println("->WB End"); // 串口打印字符串"->WB End"
g_array[2] = g_count;//储存 1s 内的红光通过滤波器时，TCS3200 输出的脉冲个数
TSC_WB(HIGH, LOW); // 选择无滤波器模式
break; */
default:
g_count = 0;//计数器清零
break;
}
}
//设置反射光中红、绿、蓝三色光分别通过滤波器时如何处理数据的标志
//该函数被 TSC_Callback( )调用
void TSC_WB(int Level0, int Level1)
{
g_count = 0;//计数值清零
g_flag ++;//输出信号计数标志
TSC_FilterColor(Level0, Level1);//滤波器模式
Timer1.setPeriod(500000);//设置输出信号脉冲计数时长为 0.5s
}
//初始化
void setup()
{
myservo.attach(9); // 该舵机由arduino第九脚控制
TSC_Init();
Serial.begin(9600);//打开串口并设置通信波特率为 9600
Timer1.initialize();//定时器初始化，默认触发值为 1s
Timer1.attachInterrupt(TSC_Callback);
// 设置定时器1的中断，中断调用函数为 TSC_Callback()
//设置 TCS3200 输出信号的上升沿触发中断，中断调用函数为 TSC_Count()
attachInterrupt(0, TSC_Count, RISING);//外部中断口初始0
delay(2000);// 延迟 2s,等待被测试物体红、绿、蓝三色在 1s 内的 TCS3200 输出信号脉冲计数
//通过白平衡测试，计算得到白色物体 RGB 值 255 与 1s 内三色光脉冲数的 RGB 比例因子
for(int i=0; i<1; i++)
Serial.println(g_array[i]); //串口打印 g_array[i] 变量值
g_SF[0] = 255.0/ g_array[0]; //蓝色光比例因子
//g_SF[1] = 255.0/ g_array[1] ; //绿色光比例因子
//g_SF[2] = 255.0/ g_array[2] ; //红色光比例因子
//打印白平衡后的蓝、绿、红三色的 RGB 比例因子
Serial.println(g_SF[0],5);
//Serial.println(g_SF[1],5);
//Serial.println(g_SF[2],5);
//蓝、绿、红三色光分别对应的1s内TCS3200输出脉冲数乘以相应的比例因子就是RGB标准值
//打印被测物体的RGB值
for(int i=0; i<1; i++)
Serial.println(int(g_array[i] * g_SF[i]));
}
//主程序
void loop()
{
g_flag = 0;
//每获得一次被测物体RGB颜色值需时0.6s
delay(600);
//打印出被测物体B颜色值
value[0]=int(g_array[0]*g_SF[0]);//将换算后的RBG值给value
Serial.println(value[0]);
//根据蓝色数值控制舵机转向
if(value[0]<60)//五毛硬币
{myservo.write(45); // 指定舵机转向的角度 45
delay(10); // 等待10ms让舵机到达指定位置
}
else//一毛硬币
{myservo.write(135); // 指定舵机转向的角度 135
delay(10); // 等待10ms让舵机到达指定位置
}
}