春ロボ ロケット団
aa
Dependencies: mbed SpeedController Encoder
Revision 0:187c4874bf41, committed 2020-03-21
- Comitter:
- maxnagazumi
- Date:
- Sat Mar 21 03:15:45 2020 +0000
- Commit message:
- aa
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Sat Mar 21 03:15:45 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/ROBOSTEP_LIBRARY/code/Encoder/#f261038cb88f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SpeedController.lib Sat Mar 21 03:15:45 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/ROBOSTEP_LIBRARY/code/SpeedController/#ea6a4709d01d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Mar 21 03:15:45 2020 +0000
@@ -0,0 +1,219 @@
+#include "mbed.h"
+#include "EC.h"
+#include "SpeedController.h"
+#define RESOLUTION 500
+
+DigitalOut led(LED1);
+CAN can1(PB_5,PB_13);
+/*
+0 start/stop
+1 to 4 , 6 to 9
+5 stop
+10 clock move
+11 not clock move
+12 speed change
+*/
+Ec2multi ec[]= {
+ Ec2multi(PC_5,PB_2,RESOLUTION),
+ Ec2multi(PA_11,PB_1,RESOLUTION),
+ Ec2multi(PB_12,PB_15,RESOLUTION),
+ Ec2multi(PC_4,PB_14,RESOLUTION)
+}; //2逓倍用class
+
+Ec2multi ecXY[]= {
+ Ec2multi(PC_6,PB_8,RESOLUTION),
+ Ec2multi(PC_8,PB_9,RESOLUTION)
+};
+
+SpeedControl motor[]= {
+ SpeedControl(PA_5,PC_7,50,ec[0]),
+ SpeedControl(PC_9,PA_1,50,ec[1]),
+ SpeedControl(PA_10,PB_4,50,ec[2]),
+ SpeedControl(PA_9,PA_7,50,ec[3])
+};
+
+DigitalIn button(USER_BUTTON);
+
+class CAN_ticker
+{
+public:
+ CAN_ticker():x(0)
+ {
+ data[0]=0;
+ }
+ void canmsg_read()
+ {
+ CANMessage msg;
+ if(can1.read(msg)) {
+ if(msg.id == 1) {
+ x=(short)(msg.data[0]);
+ }
+ }
+ }
+ int get_xCAN()
+ {
+ return x;
+ }
+private:
+ char data[0];
+ int x;
+};
+//手動出力
+double canOmega[4]= {
+ 0,0,0,0
+};
+void ledturn()
+{
+ led=1;
+ wait(0.04);
+ led=0;
+}
+void calOmega_CAN(int canx)
+{
+ static double a=0.1;
+ static int count=0;
+ switch(canx) {
+ case 1:
+ canOmega[0]=0;
+ canOmega[1]=a;
+ canOmega[2]=0;
+ canOmega[3]=-a;
+ ledturn();
+ break;
+ case 2:
+ canOmega[0]=a*1.41;
+ canOmega[1]=a*1.41;
+ canOmega[2]=-a*1.41;
+ canOmega[3]=-a*1.41;
+ ledturn();
+ break;
+ case 3:
+ canOmega[0]=a;
+ canOmega[1]=0;
+ canOmega[2]=-a;
+ canOmega[3]=0;
+ ledturn();
+ break;
+ case 4:
+ canOmega[0]=-a*1.41;
+ canOmega[1]=a*1.41;
+ canOmega[2]=a*1.41;
+ canOmega[3]=-a*1.41;
+ ledturn();
+ break;
+ case 5:
+ for(int i=0; i<4; i++) {
+ canOmega[i]=0;
+ }
+ break;
+ case 6:
+ canOmega[0]=a*1.41;
+ canOmega[1]=-a*1.41;
+ canOmega[2]=-a*1.41;
+ canOmega[3]=a*1.41;
+ ledturn();
+ break;
+ case 7:
+ canOmega[0]=0;
+ canOmega[1]=-a;
+ canOmega[2]=0;
+ canOmega[3]=a;
+ ledturn();
+ break;
+ case 8:
+ canOmega[0]=-a*1.41;
+ canOmega[1]=-a*1.41;
+ canOmega[2]=a*1.41;
+ canOmega[3]=a*1.41;
+ ledturn();
+ break;
+ case 9:
+ canOmega[0]=-a;
+ canOmega[1]=0;
+ canOmega[2]=a;
+ canOmega[3]=0;
+ ledturn();
+ break;
+ case 10:
+ for(int i=0; i<4; i++) {
+ canOmega[i]=a;
+ }
+ break;
+ case 11:
+ for(int i=0; i<4; i++) {
+ canOmega[i]=-a;
+ }
+ break;
+ case 12:
+ wait(0.2);
+ if(count==1) {
+ a=0.1;
+ count=0;
+ } else {
+ a=0.05;
+ count=1;
+ }
+ ledturn();
+ break;
+
+ }
+ for(int i=0; i<4; i++) {
+ if(canOmega[i]>0) {
+ canOmega[i]=canOmega[i]*0.8;
+ }
+ motor[i].turn(canOmega[i]);
+ }
+}
+
+//ticker に入れる関数
+Ticker canTicker;
+
+CAN_ticker canx;
+void ticker_CanRead()
+{
+ canx.canmsg_read();
+}
+
+int main()
+{
+ can1.frequency(1000000);
+ printf("start\r\n");
+ motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
+ motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
+ motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
+ motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);
+
+ motor[0].setDutyLimit(0.4);
+ motor[1].setDutyLimit(0.4);
+ motor[2].setDutyLimit(0.4);
+ motor[3].setDutyLimit(0.4);
+
+ motor[0].setPDparam( 0.01790, 0.00560);
+ motor[1].setPDparam( 0.01705, 0.00620);
+ motor[2].setPDparam( 0.01790, 0.00620);
+ motor[3].setPDparam( 0.01680, 0.00560);
+
+ int canX=20;//can変数
+ canTicker.attach(&ticker_CanRead,0.05);//can読み込み
+ while(1) {
+ printf("waiting\r\n");
+ canX=canx.get_xCAN();//０で手\動化
+ if(canX==13) {//手動化
+ wait(1);
+ canX=5;
+ while(1) {
+ printf("go\r\n");
+ calOmega_CAN(canX);
+ if(canX==13) {
+ wait(0.5);
+ break;
+ }
+ if(canX==14) {
+ printf("reset\r\n");
+ NVIC_SystemReset();
+ }
+ canX=canx.get_xCAN();
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Mar 21 03:15:45 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file