Naito Takato / Mbed 2 deprecated mbed_kudou_roller

kudou_purppo

Dependencies:   SBUS_STM32 can_network_motordriver mbed

main.cpp@2:b9291c3e7c58, 2022-09-14 (annotated)

Committer:
takatonaito
Date:
Wed Sep 14 01:55:13 2022 +0000
Revision:
2:b9291c3e7c58
Parent:
1:09e43dcf94b8 
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kobayashitakumi 0:b7116758e952 1 //送信
kobayashitakumi 0:b7116758e952 2 #include "mbed.h"
kobayashitakumi 0:b7116758e952 3 #include "sbus.h"
kobayashitakumi 0:b7116758e952 4 #include "can_network_motordriver_definition.h"
kobayashitakumi 0:b7116758e952 5
kobayashitakumi 0:b7116758e952 6
kobayashitakumi 0:b7116758e952 7 CAN pinName(p30,p29);//(rd,td)
kobayashitakumi 0:b7116758e952 8 CAN can(p9,p10);
kobayashitakumi 0:b7116758e952 9
kobayashitakumi 0:b7116758e952 10 CANMessage bluemd;//青色md
kobayashitakumi 0:b7116758e952 11 CANMessage servo;//サーボ
kobayashitakumi 0:b7116758e952 12 CANMessage load;//装填ステッピング
kobayashitakumi 0:b7116758e952 13
kobayashitakumi 0:b7116758e952 14
kobayashitakumi 0:b7116758e952 15 Serial pc(USBTX,USBRX,115200);
kobayashitakumi 0:b7116758e952 16 Sbus sbus(p13,p14);
kobayashitakumi 0:b7116758e952 17 //DigitalOut fan(p23);//p23
kobayashitakumi 0:b7116758e952 18 PwmOut fan(p23);
kobayashitakumi 0:b7116758e952 19 Ticker get_date;
kobayashitakumi 0:b7116758e952 20 Ticker controlCycle;
kobayashitakumi 0:b7116758e952 21 int a,b,c,d,e,f,g,h;
kobayashitakumi 0:b7116758e952 22 float rx,ry,lx,ly,vr,V1,V2,V3,V4;
kobayashitakumi 0:b7116758e952 23 Ticker timer;
kobayashitakumi 0:b7116758e952 24
takatonaito 1:09e43dcf94b8 25 union vr_data{float fvr;char cvr[4];};
takatonaito 1:09e43dcf94b8 26 vr_data VRer;
kobayashitakumi 0:b7116758e952 27
takatonaito 1:09e43dcf94b8 28 union ry_data{float fry;char cry[4];};
takatonaito 1:09e43dcf94b8 29 ry_data Data2;
takatonaito 1:09e43dcf94b8 30
takatonaito 1:09e43dcf94b8 31 union rx_data{float frx;char crx[4];};
takatonaito 1:09e43dcf94b8 32 rx_data RX;
kobayashitakumi 0:b7116758e952 33
kobayashitakumi 0:b7116758e952 34
kobayashitakumi 0:b7116758e952 35 void puropo();
kobayashitakumi 0:b7116758e952 36 void send();
kobayashitakumi 0:b7116758e952 37 void mainLoop();
kobayashitakumi 0:b7116758e952 38
kobayashitakumi 0:b7116758e952 39
kobayashitakumi 0:b7116758e952 40
kobayashitakumi 0:b7116758e952 41 int main() {
kobayashitakumi 0:b7116758e952 42 timer.attach(&send,0.05);//canのタイマ割込み
kobayashitakumi 0:b7116758e952 43 get_date.attach(&puropo,0.01f);//プロポのタイマ割込み
takatonaito 1:09e43dcf94b8 44 sbus.setup(Sbus::VR,Sbus::SW_D,Sbus::SW_C,Sbus::SW_B,Sbus::SW_E,Sbus::SW_A);
kobayashitakumi 0:b7116758e952 45 pinName.frequency(250000);
kobayashitakumi 0:b7116758e952 46 fan.period(0.00005f);
kobayashitakumi 0:b7116758e952 47 fan.pulsewidth(0.020);
kobayashitakumi 0:b7116758e952 48
kobayashitakumi 0:b7116758e952 49 can.frequency(500000);
kobayashitakumi 0:b7116758e952 50 controlCycle.attach(&mainLoop,0.01f);
kobayashitakumi 0:b7116758e952 51
kobayashitakumi 0:b7116758e952 52 bluemd.id = 0x103;
kobayashitakumi 0:b7116758e952 53 for(int i=0;i<8;i++) bluemd.data[i] = (int)i;
kobayashitakumi 0:b7116758e952 54 bluemd.len = 8;
kobayashitakumi 0:b7116758e952 55 bluemd.type = CANData;
kobayashitakumi 0:b7116758e952 56 bluemd.format = CANStandard;
kobayashitakumi 0:b7116758e952 57
kobayashitakumi 0:b7116758e952 58 servo.id = 0x202;
kobayashitakumi 0:b7116758e952 59 for(int i=0;i<8;i++) servo.data[i] = (int)i;
kobayashitakumi 0:b7116758e952 60 servo.len = 8;
kobayashitakumi 0:b7116758e952 61 servo.type = CANData;
kobayashitakumi 0:b7116758e952 62 servo.format = CANStandard;
kobayashitakumi 0:b7116758e952 63
kobayashitakumi 0:b7116758e952 64 load.id = 0x302;
kobayashitakumi 0:b7116758e952 65 for(int i=0;i<8;i++) load.data[i] = (int)i;
kobayashitakumi 0:b7116758e952 66 load.len = 8;
kobayashitakumi 0:b7116758e952 67 load.type = CANData;
kobayashitakumi 0:b7116758e952 68 load.format = CANStandard;
kobayashitakumi 0:b7116758e952 69
kobayashitakumi 0:b7116758e952 70 while(1) {
kobayashitakumi 0:b7116758e952 71 //fan = 1;
kobayashitakumi 0:b7116758e952 72 //fan.write(0.8);
takatonaito 1:09e43dcf94b8 73 if(d == 2&& a == 2){
kobayashitakumi 0:b7116758e952 74 /*V1=((-lx)+(ly))/2.0f;
kobayashitakumi 0:b7116758e952 75 V2=((-lx)+(-ly))/2.0f;
kobayashitakumi 0:b7116758e952 76 V3=((lx)+(-ly))/2.0f;
kobayashitakumi 0:b7116758e952 77 V4=((lx)+(ly))/2.0f;*/
kobayashitakumi 0:b7116758e952 78 V1=((-lx)+(ly))/5.0f;
kobayashitakumi 0:b7116758e952 79 V2=((-lx)+(-ly))/5.0f;
kobayashitakumi 0:b7116758e952 80 V3=((lx)+(-ly))/5.0f;
kobayashitakumi 0:b7116758e952 81 V4=((lx)+(ly))/5.0f;
takatonaito 1:09e43dcf94b8 82 }else if(d == 2){
kobayashitakumi 0:b7116758e952 83 V2 = 0.1;
kobayashitakumi 0:b7116758e952 84 V1 = 0.1;
kobayashitakumi 0:b7116758e952 85 V3 = 0.1;
kobayashitakumi 0:b7116758e952 86 V4 = 0.1;
takatonaito 1:09e43dcf94b8 87 }else if(a == 2){
kobayashitakumi 0:b7116758e952 88 V2 = -0.1;
kobayashitakumi 0:b7116758e952 89 V1 = -0.1;
kobayashitakumi 0:b7116758e952 90 V3 = -0.1;
kobayashitakumi 0:b7116758e952 91 V4 = -0.1;
kobayashitakumi 0:b7116758e952 92 }
takatonaito 1:09e43dcf94b8 93
takatonaito 1:09e43dcf94b8 94
takatonaito 1:09e43dcf94b8 95 //pc.printf("%f",Data2.frx);
takatonaito 1:09e43dcf94b8 96 pc.printf("A = %d' F= %d'H = %d'D = %d'e = %d'E = %d'vr = %f",a,b,c,d,e,f,vr);
kobayashitakumi 0:b7116758e952 97 //pc.printf("%f",Data1.flx);
kobayashitakumi 0:b7116758e952 98 pc.printf("\n\r");
kobayashitakumi 0:b7116758e952 99 }
kobayashitakumi 0:b7116758e952 100 }
kobayashitakumi 0:b7116758e952 101
kobayashitakumi 0:b7116758e952 102 void puropo(){
kobayashitakumi 0:b7116758e952 103
kobayashitakumi 0:b7116758e952 104 a=sbus.getSwitchValue(0);
kobayashitakumi 0:b7116758e952 105 b=sbus.getSwitchValue(1);
kobayashitakumi 0:b7116758e952 106 c=sbus.getSwitchValue(2);
kobayashitakumi 0:b7116758e952 107 d=sbus.getSwitchValue(3);
kobayashitakumi 0:b7116758e952 108 e=sbus.getSwitchValue(4);
kobayashitakumi 0:b7116758e952 109 f=sbus.getSwitchValue(5);
takatonaito 1:09e43dcf94b8 110
kobayashitakumi 0:b7116758e952 111
kobayashitakumi 0:b7116758e952 112
kobayashitakumi 0:b7116758e952 113
takatonaito 1:09e43dcf94b8 114 RX.frx = sbus.getStickValue(0);
takatonaito 1:09e43dcf94b8 115 Data2.fry=sbus.getStickValue(1);
kobayashitakumi 0:b7116758e952 116 lx=sbus.getStickValue(2);
kobayashitakumi 0:b7116758e952 117 ly=sbus.getStickValue(3);
kobayashitakumi 0:b7116758e952 118 vr = sbus.getVolumeValue();
kobayashitakumi 0:b7116758e952 119
kobayashitakumi 0:b7116758e952 120 }
kobayashitakumi 0:b7116758e952 121
kobayashitakumi 0:b7116758e952 122 void send(){
takatonaito 1:09e43dcf94b8 123 memcpy(load.data,Data2.cry,4);
kobayashitakumi 0:b7116758e952 124 pinName.write(load);
takatonaito 1:09e43dcf94b8 125
takatonaito 1:09e43dcf94b8 126 VRer.fvr=vr;
takatonaito 1:09e43dcf94b8 127 memcpy(bluemd.data,VRer.cvr,4);
takatonaito 1:09e43dcf94b8 128 pinName.write(bluemd);
takatonaito 1:09e43dcf94b8 129
takatonaito 1:09e43dcf94b8 130 //memcpy(load.data,RX.crx,4);
takatonaito 2:b9291c3e7c58 131 load.data[4]=RX.crx[0];
takatonaito 2:b9291c3e7c58 132 load.data[5]=RX.crx[1];
takatonaito 2:b9291c3e7c58 133 load.data[6]=RX.crx[2];
takatonaito 2:b9291c3e7c58 134 load.data[7]=RX.crx[3];
takatonaito 2:b9291c3e7c58 135 pinName.write(load);
takatonaito 2:b9291c3e7c58 136
takatonaito 1:09e43dcf94b8 137
takatonaito 1:09e43dcf94b8 138 if(c == 2){
takatonaito 1:09e43dcf94b8 139 servo.data[5]=6;
kobayashitakumi 0:b7116758e952 140 }
takatonaito 1:09e43dcf94b8 141 else{bluemd.data[5]=0;}
takatonaito 1:09e43dcf94b8 142
takatonaito 1:09e43dcf94b8 143
kobayashitakumi 0:b7116758e952 144 pinName.write(servo);
kobayashitakumi 0:b7116758e952 145 }
kobayashitakumi 0:b7116758e952 146
kobayashitakumi 0:b7116758e952 147 void mainLoop() {
kobayashitakumi 0:b7116758e952 148 static int a = 0;
kobayashitakumi 0:b7116758e952 149 switch(a){
kobayashitakumi 0:b7116758e952 150 case 0:
kobayashitakumi 0:b7116758e952 151 // motordriverTransmitMessage(&can,MD_1_MCU,md_can_flame::SEND_CONTROL_DUTY,V1);
kobayashitakumi 0:b7116758e952 152 motordriverTransmitMessage(&can,MD_1_MCU,md_can_flame::SEND_CONTROL_DUTY,V4);
kobayashitakumi 0:b7116758e952 153 break;
kobayashitakumi 0:b7116758e952 154 case 1:
kobayashitakumi 0:b7116758e952 155 // motordriverTransmitMessage(&can,MD_2_MCU,md_can_flame::SEND_CONTROL_DUTY,V2);
kobayashitakumi 0:b7116758e952 156 motordriverTransmitMessage(&can,MD_2_MCU,md_can_flame::SEND_CONTROL_DUTY,V1);
kobayashitakumi 0:b7116758e952 157 break;
kobayashitakumi 0:b7116758e952 158 case 2:
kobayashitakumi 0:b7116758e952 159 //motordriverTransmitMessage(&can,MD_3_MCU,md_can_flame::SEND_CONTROL_DUTY,V3);
kobayashitakumi 0:b7116758e952 160 motordriverTransmitMessage(&can,MD_3_MCU,md_can_flame::SEND_CONTROL_DUTY,V2);
kobayashitakumi 0:b7116758e952 161 break;
kobayashitakumi 0:b7116758e952 162 case 3:
kobayashitakumi 0:b7116758e952 163 //motordriverTransmitMessage(&can,MD_4_MCU,md_can_flame::SEND_CONTROL_DUTY,V4);
kobayashitakumi 0:b7116758e952 164 motordriverTransmitMessage(&can,MD_4_MCU,md_can_flame::SEND_CONTROL_DUTY,V3);
kobayashitakumi 0:b7116758e952 165 break;
kobayashitakumi 0:b7116758e952 166 /* case 4:
kobayashitakumi 0:b7116758e952 167 motordriverTransmitMessage(&can,MD_5_MCU,md_can_flame::SEND_CONTROL_DUTY,0.8);
kobayashitakumi 0:b7116758e952 168 break;*/
kobayashitakumi 0:b7116758e952 169 default:
kobayashitakumi 0:b7116758e952 170 break;
kobayashitakumi 0:b7116758e952 171 }
kobayashitakumi 0:b7116758e952 172 if(a >= 4){
kobayashitakumi 0:b7116758e952 173 a = 0;
kobayashitakumi 0:b7116758e952 174 }else{
kobayashitakumi 0:b7116758e952 175 a++;
kobayashitakumi 0:b7116758e952 176 }
kobayashitakumi 0:b7116758e952 177 }