2020_ROBOCON
motor(lpc1768)
Dependencies: mbed PololuLedStrip
Diff: main.cpp
- Revision:
- 0:3b06c3283e31
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Jan 25 07:20:38 2021 +0000
@@ -0,0 +1,384 @@
+#include "mbed.h"
+#include "PololuLedStrip.h"
+#include <math.h>
+
+Serial Xbee(P0_10,P0_11,19200); //TX,RX,クロックレート
+Serial Xbee2(P0_0,P0_1,19200); //ＬＥＤに送信
+Serial pc(USBTX,USBRX); //pc(USBTX,USBRX)
+Ticker ticker;
+unsigned char RX_data;
+
+#define DIGITS 8
+#define ledsize 65
+//円周率
+#define PI 3.14159265358979
+//モータードライバ
+#define M1 0x01
+
+//モータードライバへの命令
+#define STOP 0
+#define CW 1
+#define CCW -1
+#define bSTOP 10
+#define bCW 11
+#define bCCW -11
+
+#define sCW 21
+#define sCCW -21
+#define sSTOP 20
+#define shCW 31
+#define shCCW -31
+
+#define lCW 51
+#define lCCW -51
+#define lSTOP 50
+#define lsCW 61
+#define lsCCW -61
+#define lsSTOP 60
+
+#define rON 41
+#define rOFF -41
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut ledx(LED4);
+I2C md_i2c_tx(P0_27, P0_28);
+
+void cv2bin( unsigned char n ) {
+ int i;
+
+ for( i = DIGITS-1; i >= 0; i-- ) {
+ printf( "%d", ( n >> i ) & 1 );
+ }
+ printf( "\n" );
+}
+
+//i2c通信
+void md_setoutput(unsigned char address, int rotate, int duty) {
+ char val;
+ char data[2];
+ /* H30新型：処理最適化 */
+ switch (rotate) {
+ case CW: data[0] = 'F'; break;// 電圧比例制御モード（PWMがLの区間はフリー）
+ case CCW: data[0] = 'R'; break;
+ case STOP: data[0] = 'S'; break;
+
+ case bCW: data[0] = 'f'; break; // 電圧比例制御モード（PWMがLの区間はブレーキ） //ブレーキ停止は機体に負荷がかかるため使用禁止
+ case bCCW: data[0] = 'r'; break;
+ case bSTOP: data[0] = 's'; break;
+
+ case sCW: data[0] = 'A'; break; // 速度比例制御モード
+ case sCCW: data[0] = 'B'; break;
+ case sSTOP: data[0] = 'T'; break;
+
+ case shCW: data[0] = 'a'; break; // 速度比例制御モード（命令パケット最上位ビットに指令値9ビット目を入れることで分解能2倍）
+ case shCCW: data[0] = 'b'; break;
+
+ case lCW: data[0] = 'L'; break; // 通常LAP
+ case lCCW: data[0] = 'M'; break;
+ case lSTOP: data[0] = 'N'; break;
+ case lsCW: data[0] = 'l'; break; // 速調LAP
+ case lsCCW: data[0] = 'm'; break;
+ case lsSTOP: data[0] = 'n'; break;
+
+ case rON: data[0] = 'P'; break; //リレー駆動モード
+ case rOFF: data[0] = 'p'; break;
+ }
+
+ data[1] = duty & 0xFF;
+
+ val = md_i2c_tx.write(address << 1, data, 2); //duty送信
+}
+#define LED_COUNT 66 //LEDの数
+PololuLedStrip ledStrip1(P2_13);
+rgb_color colors[LED_COUNT];
+int cnt = 0;
+int EMS = 0;
+int con = 0;
+int motorOn = 0;
+int LEDDATA = 1;
+
+void Xbee_RX(){
+
+ RX_data = Xbee.getc(); //Xbeeから送られてきた文字を代入
+ pc.putc(RX_data); //値をpcに表示
+
+ //非常停止 EMSが１で入る
+ if (cnt == 0 && RX_data == 'S'){
+ cnt++;
+ }else if (cnt == 1 && RX_data == 'T'){
+ cnt++;
+ }else if (cnt == 2 && RX_data == 'O'){
+ cnt++;
+ }else if (cnt == 3 && RX_data == 'P'){
+ cnt = 0;
+ EMS = 1;
+ led1 = 1;
+ }else if (RX_data == 'L'){
+ cnt = 0;
+ EMS = 0;
+ led1 = 0;
+ }
+}
+
+rgb_color COLOR(float H,int n){
+
+ float r = 0, g = 0, b = 0;
+ H = 100; //光の強さ
+ switch (n){
+ case 1:r = H; g = 0; b = 0; break; //赤
+ case 2:r = 0; g = H; b = 0; break; //緑
+ case 3:r = 0; g = 0; b = H; break; //青
+ case 4:r = H; g = H; b = 0; break; //黄色
+ case 5:r = 0; g = H; b = H; break; //水色
+ case 6:r = H; g = 0; b = H; break; //紫
+ case 7:r = H; g = H; b = H; break; //白
+ case 8:r = H; g = H * 0.5; b = 0; break; //オレンジ
+ }
+ return (rgb_color){r,g,b};
+}
+
+//フォトリフレクタ
+double fot;
+int h = 0;
+int b = 0;
+//↓LED色
+int R = 1;
+int G = 2;
+int B = 3;
+int Y = 4;
+int L = 5;
+int P = 6;
+int W = 7;
+int O = 8;
+
+void ledsw(){
+ h++;
+}
+
+int main() {
+ wait_ms(10);
+ led1 = 1;
+ led2 = 0;
+ led3 = 0;
+ ledx = 1;
+ Xbee.attach(&Xbee_RX, Serial::RxIrq); //割り込み
+ pc.baud(115200); //pcクロックレート
+ md_i2c_tx.frequency(20000000UL/(16 + 2 * 2 * 16));//I2C通信
+
+ while(1){
+ //機構用モータ
+ if(EMS){
+ md_setoutput(M1,STOP,0);
+ }else if(EMS == 0){
+ md_setoutput(M1,CW,255);
+ }
+
+ //***************打ち上げ処理******************//
+ if(RX_data == 'A' && con == 0){
+ con = 1;
+ h = 0;
+ ticker.attach(ledsw,0.05);
+ }else if(RX_data == 'B' && con == 1){
+ con = 2;
+ h = 0;
+ ticker.attach(ledsw,0.2);
+ }else if(RX_data == 'C' && con == 2){
+ con = 3;
+ h = 0;
+ ticker.attach(ledsw,0.2);
+ }else if(RX_data == 'D' && con == 3){
+ con = 4;
+ h = 0;
+ ticker.attach(ledsw,0.2);
+ }else if(RX_data == 'E' && con == 4){
+ con = 5;
+ h = 0;
+ ticker.attach(ledsw,0.2);
+ }else if(RX_data == 'A' && con == 5){
+ con = 6;
+ h = 0;
+ ticker.attach(ledsw,0.2);
+ }else if(RX_data == 'B' && con == 6){
+ con = 7;
+ h = 0;
+ ticker.attach(ledsw,0.2);
+ }else if(RX_data == 'C' && con == 7){
+ con = 8;
+ h = 0;
+ ticker.attach(ledsw,0.2);
+ }else if(RX_data == 'D' && con == 8){
+ con = 9;
+ h = 0;
+ ticker.attach(ledsw,0.2);
+ }
+
+ if(con == 1){
+ if(h < LED_COUNT + 1){
+ for(int k = 0; k < LED_COUNT; k++){
+ uint8_t phase =(k << 1);
+ colors[k] = COLOR(phase, 0);
+ }
+
+ uint8_t phase =(h << 1);
+ colors[ledsize - h] = COLOR(phase, B);
+ ledStrip1.write(colors, LED_COUNT);
+
+ if(h == LED_COUNT + 1){
+ ticker.detach();
+ Xbee2.putc('A');
+ }
+ }
+ }
+
+ if(con == 2){
+ if(h < LED_COUNT + 1){
+ for(int k = 0; k < LED_COUNT; k++){
+ uint8_t phase =(k << 1);
+ colors[k] = COLOR(phase, 0);
+ }
+
+ uint8_t phase =(h << 1);
+ colors[ledsize - h] = COLOR(phase, G);
+ ledStrip1.write(colors, LED_COUNT);
+
+ if(h == LED_COUNT + 1){
+ ticker.detach();
+ Xbee2.putc('B');
+ }
+ }
+ }
+
+ if(con == 3){
+ if(h < LED_COUNT + 1){
+ for(int k = 0; k < LED_COUNT; k++){
+ uint8_t phase =(k << 1);
+ colors[k] = COLOR(phase, 0);
+ }
+
+ uint8_t phase =(h << 1);
+ colors[ledsize - h] = COLOR(phase, R);
+ ledStrip1.write(colors, LED_COUNT);
+
+ if(h == LED_COUNT + 1){
+ ticker.detach();
+ Xbee2.putc('C');
+ }
+ }
+ }
+
+ if(con == 4){
+ if(h < LED_COUNT + 1){
+ for(int k = 0; k < LED_COUNT; k++){
+ uint8_t phase =(k << 1);
+ colors[k] = COLOR(phase, 0);
+ }
+
+ uint8_t phase =(h << 1);
+ colors[ledsize - h] = COLOR(phase, O);
+ ledStrip1.write(colors, LED_COUNT);
+
+ if(h == LED_COUNT + 1){
+ ticker.detach();
+ Xbee2.putc('D');
+ }
+ }
+ }
+
+ if(con == 5){
+ if(h < LED_COUNT + 1){
+ for(int k = 0; k < LED_COUNT; k++){
+ uint8_t phase =(k << 1);
+ colors[k] = COLOR(phase, 0);
+ }
+
+ uint8_t phase =(h << 1);
+ colors[ledsize - h] = COLOR(phase, P);
+ ledStrip1.write(colors, LED_COUNT);
+
+ if(h == LED_COUNT + 1){
+ ticker.detach();
+ Xbee2.putc('E');
+ }
+ }
+ }
+
+ if(con == 6){
+ if(h < LED_COUNT + 1){
+ for(int k = 0; k < LED_COUNT; k++){
+ uint8_t phase =(k << 1);
+ colors[k] = COLOR(phase, 0);
+ }
+
+ uint8_t phase =(h << 1);
+ colors[ledsize - h] = COLOR(phase, P);
+ ledStrip1.write(colors, LED_COUNT);
+
+ if(h == LED_COUNT + 1){
+ ticker.detach();
+ Xbee2.putc('F');
+ }
+ }
+ }
+
+ if(con == 7){
+ if(h < LED_COUNT + 1){
+ for(int k = 0; k < LED_COUNT; k++){
+ uint8_t phase =(k << 1);
+ colors[k] = COLOR(phase, 0);
+ }
+
+ uint8_t phase =(h << 1);
+ colors[ledsize - h] = COLOR(phase, P);
+ ledStrip1.write(colors, LED_COUNT);
+
+
+ if(h == LED_COUNT + 1){
+ ticker.detach();
+ Xbee2.putc('G');
+ }
+ }
+ }
+
+ if(con == 8){
+ if(h < LED_COUNT + 1){
+ for(int k = 0; k < LED_COUNT; k++){
+ uint8_t phase =(k << 1);
+ colors[k] = COLOR(phase, 0);
+ }
+
+ uint8_t phase =(h << 1);
+ colors[ledsize - h] = COLOR(phase, P);
+ ledStrip1.write(colors, LED_COUNT);
+
+
+ if(h == LED_COUNT + 1){
+ ticker.detach();
+ Xbee2.putc('H');
+ }
+ }
+ }
+
+ if(con == 9){
+ if(h < LED_COUNT + 1){
+ for(int k = 0; k < LED_COUNT; k++){
+ uint8_t phase =(k << 1);
+ colors[k] = COLOR(phase, 0);
+ }
+
+ uint8_t phase =(h << 1);
+ colors[ledsize - h] = COLOR(phase, P);
+ ledStrip1.write(colors, LED_COUNT);
+
+ if(h == LED_COUNT + 1){
+ ticker.detach();
+ Xbee2.putc('I');
+ con = 0;
+ }
+ }
+ }
+
+ ledStrip1.write(colors, LED_COUNT);
+ }
+}
\ No newline at end of file