Ryo Ogata
aaaa
Dependencies: ColorSensor1 Servo mbed
Diff: main.cpp
- Revision:
- 0:4c7c138f3891
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Oct 19 04:50:26 2013 +0000
@@ -0,0 +1,269 @@
+//カラーセンサ6つ同時に動かすプログラム
+//大会用ロボ
+//ジャンプのプログラムも入ってる
+//10月17日に使う
+#include "mbed.h"
+#include "ColorSensor.h"
+#include "Servo.h"
+
+#include "main.h"
+
+
+
+
+int cAve,roboF, jumI;
+
+void tic_sensor()
+{
+ colorUpdate(1);
+ roboF = robotFront();
+ jumI = jumpInstruction(roboF);
+ //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+ /*lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+ */
+}
+
+
+
+////////////////////////////////////////カラーセンサの////////////////////////////////////////
+////////////////////////////////////////更新と補正プログラム///////////////////////////////////
+/*void rivisedate()
+{
+ unsigned long red = 0,green = 0,blue =0;
+ static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
+
+ //最初の20回だけ平均を取る
+ for (int i=0;i<=20;i++){
+ color0.getRGB(R[0],G[0],B[0]);
+ red += R[0] ;
+ green += G[0] ;
+ blue += B[0] ;
+ //pc.printf(" %d %d\n",ptm(sum),sum);
+ }
+
+ rir = (double)green/ red ;
+ rib = (double)green/ blue ;
+}*/
+
+void colorUpdate(uint8_t mode)
+{
+ double colorSum[COLOR_NUM];
+ unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
+
+
+ for (int i=0; i<COLOR_NUM; i++){
+ R[i] = 0;
+ G[i] = 0;
+ B[i] = 0;
+ redp[i] = 0;
+ greenp[i] = 0;
+ bluep[i] = 0;
+ }
+
+ //カラーセンサの切り替え
+ if(mode == 1){
+ color0.getRGB(R[0],G[0],B[0]);
+ color1.getRGB(R[1],G[1],B[1]);
+ color2.getRGB(R[2],G[2],B[2]);
+
+ color3.getRGB(R[3],G[3],B[3]);
+ color4.getRGB(R[4],G[4],B[4]);
+ color5.getRGB(R[5],G[5],B[5]);
+ }
+
+ /*for (int i=0; i<COLOR_NUM; i++){
+ colorSum[i] = R[i]*rir + G[i] + B[i]*rib ;
+ redp[i] = R[i]* rir * 100 / colorSum[i];
+ greenp[i] = G[i] * 100 / colorSum[i];
+ bluep[i] = B[i]* rib * 100 / colorSum[i];
+ }*/
+
+ for (int i=0; i<COLOR_NUM; i++){
+ colorSum[i] = R[i]*0.65 + G[i] + B[i] *1.3;
+ redp[i] = R[i]*0.65 * 100 / colorSum[i];
+ greenp[i] = G[i] * 100 / colorSum[i];
+ bluep[i] = B[i]*1.3 * 100 / colorSum[i];
+ }
+}
+
+
+uint16_t moving_ave(uint8_t num, uint16_t data)//移動平均的な
+{
+ uint8_t threshold = 0;
+ static uint16_t tmp[COLOR_NUM][10] = {{0}};
+ //static uint32_t sum[COLOR_NUM] = {0};
+
+ //sum[num] -= tmp[num][9];
+ //sum[num] += data;
+
+ for(uint8_t i=9; i>=1; i--){
+ tmp[num][i] = tmp[num][i-1];
+ }
+ tmp[num][0] = data;
+
+ for(uint8_t i=0; i<=9; i++){
+ if(tmp[num][i] >= B_THR){
+ threshold++;
+ }
+ }
+
+ return threshold;
+}
+
+////////////////////////////////////////ロボの状態///////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////
+uint8_t robotFront()//縄回しロボの正面にいるか
+{
+ uint8_t threshold = 0;
+
+ for(int i=0; i<COLOR_NUM; i++){
+ if(moving_ave(i, bluep[i]) >= 8) threshold++;
+ }
+
+
+ /*for(int i=0; i<COLOR_NUM; i++){
+ cAve = moving_ave(i, bluep[i]);
+ if(cAve >= 8) threshold++;
+ }*/
+
+ if(threshold >= 1){
+ return 1;
+ }else{
+ return 0;
+ }
+}
+
+uint8_t jumpInstruction(uint8_t front)//ジャンプ命令
+{
+ uint8_t threshold = 0;
+
+ if(front == 1){
+ for(int i=0; i<COLOR_NUM; i++){
+ if(redp[i] >= R_THR) threshold++;
+ }
+ }
+
+ if(threshold >= 1){
+ return 1;
+ }else{
+ return 0;
+ }
+}
+
+
+uint8_t robotoStop()
+{
+ uint8_t threshold = 0;
+
+ for(int i=0; i<COLOR_NUM; i++){
+ if(bluep[i] >= B_THR) threshold++;
+ }
+
+ if(threshold >= 1){
+ return 1;
+ }else{
+ return 0;
+ }
+}
+
+
+////////////////////////////////////////ジャンププログラム////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////
+/*
+void jumping(uint8_t threshold)
+{
+ //超音波でジャンプのタイミング合わせる
+ if(threshold >= 1){
+ jump_t.reset();
+ jump_t.start();
+ while(ultrasonicVal[0] < 1700){
+ led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0;
+ air[0] = 1; air[1] = 0;
+
+ if(jump_t.read_ms() > 1000)break;
+ }
+ led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1;
+ air[0] = 0; air[1] = 1;
+ wait(0.5);
+ }else{
+ led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
+ }
+}*/
+
+/*void jumpAction(uint8_t threshold)
+{
+ if(sw1){//フォトインタラプタがオフ
+ myservo1 = 0.3; led[0] = 0;led[1] = 1; led[2] = 0; led[3] = 0;
+ }else{//フォトインタラプタがオン
+ myservo1 = 0.5; led[0] = 1;led[1] = 0; led[2] = 0; led[3] = 0;
+ }
+
+ if(threshold){
+ led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
+ myservo1 = 0.3;
+ wait(0.2);
+ }else{
+ //led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
+ }
+
+}*/
+
+
+
+
+
+
+#define SEND_DATA_NUM 7
+#define RECEIVE_DATA_NUM 7
+
+#define KEYCODE 120
+
+Serial device(p28,p27);
+
+static uint8_t SendData0[SEND_DATA_NUM];
+
+void dev_tx()
+{
+ static uint8_t count = 0;
+
+ SendData0[0] = KEYCODE;
+
+ device.putc(SendData0[count]);
+
+ count++;
+
+ if(count > SEND_DATA_NUM){
+ count = 0;
+ }
+
+ led[4] = 1;
+}
+
+
+
+
+
+int main() {
+ //init
+
+ /*device.baud(9600);
+ device.printf("START");
+ device.attach(&dev_tx,Serial::TxIrq);*/
+
+ //rivisedate();
+ wait(3);
+
+ interrupt0.attach(&tic_sensor, 0.1/*sec*/);//0.04sec以上じゃないとmain動かない?
+ //sw1.mode(PullUp);
+ //init end
+
+ while(1) {
+ SendData0[1] = jumI;
+
+ pc.printf("R:%d G:%d B:%d %t moving_ave():%d %t robotFront():%d %t jumpInstruction():%d\n", redp[2], greenp[2], bluep[2], cAve, roboF, jumI);
+
+ //jumpAction(jumI);
+ }
+}