ロボステ6期
12/18
Diff: main.cpp
- Revision:
- 0:6e2abd0956f1
- Child:
- 1:bc34fdc4e16b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Dec 14 12:42:13 2019 +0000
@@ -0,0 +1,250 @@
+#include "mbed.h"
+#include "EC.h" //Encoderライブラリをインクルード
+#include "SpeedController.h"
+#define RESOLUTION 2048//分解能
+
+//#define NHK2020_TEST_MODE
+#define NHK2020_MAIN_MODE
+
+CAN can1(p30,p29);
+
+Ticker can_ticker; //can用ticker
+
+Ec4multi EC_backdrop(p15,p16,RESOLUTION);
+SpeedControl backdrop(p21,p22,50,EC_backdrop);
+Serial pc(USBTX,USBRX);
+DigitalOut snatch(p8);
+DigitalOut pass1(p27);
+DigitalOut pass2(p28);
+//Ticker motor_tick; //角速度計算用ticker
+
+char t2[1]= {0}; //動作番号（送信する値（char型））
+int t2_r=0, T2=0; //動作番号（受け取った値、送信する値（int型））
+
+double a=0;//now
+double b=0;//target
+double turn=0;
+int k = 0;
+void tsukami()
+{
+ b=1.3;
+}
+void put()
+{
+ b=-0.7;
+}
+void top()
+{
+ b=0;
+}
+void move()
+{
+ double old_turn=turn;
+ a=EC_backdrop.getRad();
+ if(a-b>=0.1) {
+ turn=0.1;
+ //pc.printf("F");
+
+ } else if (a-b>=0.05) {
+ turn=10*(a-b)*(a-b);
+ //pc.printf("f");
+ } else if (b-a>=0.1) {
+ turn=-0.1;
+ //pc.printf("B");
+ } else if (b-a>=0.05) {
+ turn=-10*(a-b)*(a-b);
+ //pc.printf("b");
+ } else {
+ backdrop.stop();
+ backdrop.turn(0);
+ turn=0;
+ //break;
+ //pc.printf("s");
+ }
+ if(turn*old_turn<0)turn=0;
+ backdrop.turn(turn);
+}
+
+int q = 0;
+
+void can_read()
+{
+
+
+ CANMessage msg;
+
+ if(can1.read(msg)) {
+
+ if(msg.id == 1) {
+ t2_r = msg.data[0];
+ //printf("arm T2 = %d t2_r = %d\n\r",T2,t2_r);
+ } else {
+ if(q > 100) {
+ //printf("id1 fale\n\r");
+ q = 0;
+ }
+ q++;
+ }
+
+ } else {
+ if(k > 100) {
+ //printf("arm fale\n\r");
+ k = 0;
+ }
+ k++;
+ }
+
+ t2[0] = T2;
+
+ if(can1.write(CANMessage(2,t2,1))) {
+ }
+
+ if(t2_r > T2) {
+ T2 = t2_r;
+ }
+
+
+}
+
+
+int main()
+{
+ move();
+
+ pc.printf("setting please");
+ while(1) {
+
+
+
+
+#ifdef NHK2020_TEST_MODE
+
+ pc.printf("%lf",EC_backdrop.getRad());
+ if(pc.readable()) {
+ char sel=pc.getc();
+ if(sel=='z') {
+ pc.printf("z\r\n");
+ tsukami();
+ } else if(sel=='x') {
+ pc.printf("x\r\n");
+ put();
+ } else if(sel=='c') {
+ pc.printf("c\r\n");
+ top();
+ }
+ /* if(sel=='q') {
+ printf("\r\n");
+ if(denjiben==0)denjiben=1;
+ else denjiben=0;
+ }*/
+ if(sel=='1') {
+ snatch=0;
+ printf("snatch_off\r\n");
+ }
+ if(sel=='2') {
+ snatch=1;
+ printf("snatch_on\r\n");
+ }
+ if(sel=='3') {
+ pass1=0;
+ printf("pass1_off\r\n");
+ }
+ if(sel=='4') {
+ pass1=1;
+ printf("pass1_on\r\n");
+ }
+ if(sel=='5') {
+ pass2=0;
+ printf("pass2_off\r\n");
+ }
+ if(sel=='6') {
+ pass2=1;
+ printf("pass2_on\r\n");
+ }
+ // if(sel=='a') {
+ // //pc.printf("x\r\n");
+ // //put();
+ // }
+ }
+ }
+#endif
+#ifdef NHK2020_MAIN_MODE
+ can1.frequency(1000000);
+ can_ticker.attach(&can_read,0.01);
+ printf("wait_mode\r\n");
+ while(1) {
+ char sel=pc.getc();
+ if(sel=='1') {
+ snatch=0;
+ printf("snatch_off\r\n");
+ }
+ if(sel=='2') {
+ snatch=1;
+ printf("snatch_on\r\n");
+ }
+ if(sel=='3') {
+ pass1=0;
+ printf("pass1_off\r\n");
+ }
+ if(sel=='4') {
+ pass1=1;
+ printf("pass1_on\r\n");
+ }
+ if(sel=='5') {
+ pass2=0;
+ printf("pass2_off\r\n");
+ }
+ if(sel=='6') {
+ pass2=1;
+ printf("pass2_on\r\n");
+ }
+ if(sel=='q') {
+ printf("end_wait_mode\r\n");
+ break;
+
+ }
+ }
+
+ if(T2 == 0) { //スタート位置からボール前まで移動（アーム待機）
+ T2=1;
+ while(1) {
+ //printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r);
+ wait(0.1);
+ if(T2 == 1) {
+ break;
+ }
+ }
+ }
+
+ if(T2 == 1) { //ボール掴んで投げる
+ //printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r);
+ tsukami();
+ move();
+ wait(3);
+ snatch=0;
+ wait(1);
+ put();
+ wait(3);
+ snatch=1;
+ wait(1);
+ top();
+ wait(5);
+ pass1=0;
+ wait(5);
+ //wait(5);
+
+ T2++;
+ }
+ if(T2 == 2) { //スタートゾーンに戻る（アーム待機）
+ while(1) {
+ //printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r);
+ wait(0.1);
+ if(T2 == 1) {
+ break;
+ }
+ }
+ }
+ printf("end\n\r");
+}
+#endif
+}
\ No newline at end of file