![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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
diff -r 000000000000 -r 187c4874bf41 Encoder.lib --- /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
diff -r 000000000000 -r 187c4874bf41 SpeedController.lib --- /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
diff -r 000000000000 -r 187c4874bf41 main.cpp --- /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();//0で手\動化 + 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
diff -r 000000000000 -r 187c4874bf41 mbed.bld --- /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