春ロボ　ロケット団 / Mbed 2 deprecated HaruRobo_syudou

aa

Dependencies:   mbed SpeedController Encoder

main.cpp@0:187c4874bf41, 2020-03-21 (annotated)

Committer:
maxnagazumi
Date:
Sat Mar 21 03:15:45 2020 +0000
Revision:
0:187c4874bf41
aa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maxnagazumi 0:187c4874bf41 1 #include "mbed.h"
maxnagazumi 0:187c4874bf41 2 #include "EC.h"
maxnagazumi 0:187c4874bf41 3 #include "SpeedController.h"
maxnagazumi 0:187c4874bf41 4 #define RESOLUTION 500
maxnagazumi 0:187c4874bf41 5
maxnagazumi 0:187c4874bf41 6 DigitalOut led(LED1);
maxnagazumi 0:187c4874bf41 7 CAN can1(PB_5,PB_13);
maxnagazumi 0:187c4874bf41 8 /*
maxnagazumi 0:187c4874bf41 9 0 start/stop
maxnagazumi 0:187c4874bf41 10 1 to 4 , 6 to 9
maxnagazumi 0:187c4874bf41 11 5 stop
maxnagazumi 0:187c4874bf41 12 10 clock move
maxnagazumi 0:187c4874bf41 13 11 not clock move
maxnagazumi 0:187c4874bf41 14 12 speed change
maxnagazumi 0:187c4874bf41 15 */
maxnagazumi 0:187c4874bf41 16 Ec2multi ec[]= {
maxnagazumi 0:187c4874bf41 17 Ec2multi(PC_5,PB_2,RESOLUTION),
maxnagazumi 0:187c4874bf41 18 Ec2multi(PA_11,PB_1,RESOLUTION),
maxnagazumi 0:187c4874bf41 19 Ec2multi(PB_12,PB_15,RESOLUTION),
maxnagazumi 0:187c4874bf41 20 Ec2multi(PC_4,PB_14,RESOLUTION)
maxnagazumi 0:187c4874bf41 21 }; //2逓倍用class
maxnagazumi 0:187c4874bf41 22
maxnagazumi 0:187c4874bf41 23 Ec2multi ecXY[]= {
maxnagazumi 0:187c4874bf41 24 Ec2multi(PC_6,PB_8,RESOLUTION),
maxnagazumi 0:187c4874bf41 25 Ec2multi(PC_8,PB_9,RESOLUTION)
maxnagazumi 0:187c4874bf41 26 };
maxnagazumi 0:187c4874bf41 27
maxnagazumi 0:187c4874bf41 28 SpeedControl motor[]= {
maxnagazumi 0:187c4874bf41 29 SpeedControl(PA_5,PC_7,50,ec[0]),
maxnagazumi 0:187c4874bf41 30 SpeedControl(PC_9,PA_1,50,ec[1]),
maxnagazumi 0:187c4874bf41 31 SpeedControl(PA_10,PB_4,50,ec[2]),
maxnagazumi 0:187c4874bf41 32 SpeedControl(PA_9,PA_7,50,ec[3])
maxnagazumi 0:187c4874bf41 33 };
maxnagazumi 0:187c4874bf41 34
maxnagazumi 0:187c4874bf41 35 DigitalIn button(USER_BUTTON);
maxnagazumi 0:187c4874bf41 36
maxnagazumi 0:187c4874bf41 37 class CAN_ticker
maxnagazumi 0:187c4874bf41 38 {
maxnagazumi 0:187c4874bf41 39 public:
maxnagazumi 0:187c4874bf41 40 CAN_ticker():x(0)
maxnagazumi 0:187c4874bf41 41 {
maxnagazumi 0:187c4874bf41 42 data[0]=0;
maxnagazumi 0:187c4874bf41 43 }
maxnagazumi 0:187c4874bf41 44 void canmsg_read()
maxnagazumi 0:187c4874bf41 45 {
maxnagazumi 0:187c4874bf41 46 CANMessage msg;
maxnagazumi 0:187c4874bf41 47 if(can1.read(msg)) {
maxnagazumi 0:187c4874bf41 48 if(msg.id == 1) {
maxnagazumi 0:187c4874bf41 49 x=(short)(msg.data[0]);
maxnagazumi 0:187c4874bf41 50 }
maxnagazumi 0:187c4874bf41 51 }
maxnagazumi 0:187c4874bf41 52 }
maxnagazumi 0:187c4874bf41 53 int get_xCAN()
maxnagazumi 0:187c4874bf41 54 {
maxnagazumi 0:187c4874bf41 55 return x;
maxnagazumi 0:187c4874bf41 56 }
maxnagazumi 0:187c4874bf41 57 private:
maxnagazumi 0:187c4874bf41 58 char data[0];
maxnagazumi 0:187c4874bf41 59 int x;
maxnagazumi 0:187c4874bf41 60 };
maxnagazumi 0:187c4874bf41 61 //手動出力
maxnagazumi 0:187c4874bf41 62 double canOmega[4]= {
maxnagazumi 0:187c4874bf41 63 0,0,0,0
maxnagazumi 0:187c4874bf41 64 };
maxnagazumi 0:187c4874bf41 65 void ledturn()
maxnagazumi 0:187c4874bf41 66 {
maxnagazumi 0:187c4874bf41 67 led=1;
maxnagazumi 0:187c4874bf41 68 wait(0.04);
maxnagazumi 0:187c4874bf41 69 led=0;
maxnagazumi 0:187c4874bf41 70 }
maxnagazumi 0:187c4874bf41 71 void calOmega_CAN(int canx)
maxnagazumi 0:187c4874bf41 72 {
maxnagazumi 0:187c4874bf41 73 static double a=0.1;
maxnagazumi 0:187c4874bf41 74 static int count=0;
maxnagazumi 0:187c4874bf41 75 switch(canx) {
maxnagazumi 0:187c4874bf41 76 case 1:
maxnagazumi 0:187c4874bf41 77 canOmega[0]=0;
maxnagazumi 0:187c4874bf41 78 canOmega[1]=a;
maxnagazumi 0:187c4874bf41 79 canOmega[2]=0;
maxnagazumi 0:187c4874bf41 80 canOmega[3]=-a;
maxnagazumi 0:187c4874bf41 81 ledturn();
maxnagazumi 0:187c4874bf41 82 break;
maxnagazumi 0:187c4874bf41 83 case 2:
maxnagazumi 0:187c4874bf41 84 canOmega[0]=a*1.41;
maxnagazumi 0:187c4874bf41 85 canOmega[1]=a*1.41;
maxnagazumi 0:187c4874bf41 86 canOmega[2]=-a*1.41;
maxnagazumi 0:187c4874bf41 87 canOmega[3]=-a*1.41;
maxnagazumi 0:187c4874bf41 88 ledturn();
maxnagazumi 0:187c4874bf41 89 break;
maxnagazumi 0:187c4874bf41 90 case 3:
maxnagazumi 0:187c4874bf41 91 canOmega[0]=a;
maxnagazumi 0:187c4874bf41 92 canOmega[1]=0;
maxnagazumi 0:187c4874bf41 93 canOmega[2]=-a;
maxnagazumi 0:187c4874bf41 94 canOmega[3]=0;
maxnagazumi 0:187c4874bf41 95 ledturn();
maxnagazumi 0:187c4874bf41 96 break;
maxnagazumi 0:187c4874bf41 97 case 4:
maxnagazumi 0:187c4874bf41 98 canOmega[0]=-a*1.41;
maxnagazumi 0:187c4874bf41 99 canOmega[1]=a*1.41;
maxnagazumi 0:187c4874bf41 100 canOmega[2]=a*1.41;
maxnagazumi 0:187c4874bf41 101 canOmega[3]=-a*1.41;
maxnagazumi 0:187c4874bf41 102 ledturn();
maxnagazumi 0:187c4874bf41 103 break;
maxnagazumi 0:187c4874bf41 104 case 5:
maxnagazumi 0:187c4874bf41 105 for(int i=0; i<4; i++) {
maxnagazumi 0:187c4874bf41 106 canOmega[i]=0;
maxnagazumi 0:187c4874bf41 107 }
maxnagazumi 0:187c4874bf41 108 break;
maxnagazumi 0:187c4874bf41 109 case 6:
maxnagazumi 0:187c4874bf41 110 canOmega[0]=a*1.41;
maxnagazumi 0:187c4874bf41 111 canOmega[1]=-a*1.41;
maxnagazumi 0:187c4874bf41 112 canOmega[2]=-a*1.41;
maxnagazumi 0:187c4874bf41 113 canOmega[3]=a*1.41;
maxnagazumi 0:187c4874bf41 114 ledturn();
maxnagazumi 0:187c4874bf41 115 break;
maxnagazumi 0:187c4874bf41 116 case 7:
maxnagazumi 0:187c4874bf41 117 canOmega[0]=0;
maxnagazumi 0:187c4874bf41 118 canOmega[1]=-a;
maxnagazumi 0:187c4874bf41 119 canOmega[2]=0;
maxnagazumi 0:187c4874bf41 120 canOmega[3]=a;
maxnagazumi 0:187c4874bf41 121 ledturn();
maxnagazumi 0:187c4874bf41 122 break;
maxnagazumi 0:187c4874bf41 123 case 8:
maxnagazumi 0:187c4874bf41 124 canOmega[0]=-a*1.41;
maxnagazumi 0:187c4874bf41 125 canOmega[1]=-a*1.41;
maxnagazumi 0:187c4874bf41 126 canOmega[2]=a*1.41;
maxnagazumi 0:187c4874bf41 127 canOmega[3]=a*1.41;
maxnagazumi 0:187c4874bf41 128 ledturn();
maxnagazumi 0:187c4874bf41 129 break;
maxnagazumi 0:187c4874bf41 130 case 9:
maxnagazumi 0:187c4874bf41 131 canOmega[0]=-a;
maxnagazumi 0:187c4874bf41 132 canOmega[1]=0;
maxnagazumi 0:187c4874bf41 133 canOmega[2]=a;
maxnagazumi 0:187c4874bf41 134 canOmega[3]=0;
maxnagazumi 0:187c4874bf41 135 ledturn();
maxnagazumi 0:187c4874bf41 136 break;
maxnagazumi 0:187c4874bf41 137 case 10:
maxnagazumi 0:187c4874bf41 138 for(int i=0; i<4; i++) {
maxnagazumi 0:187c4874bf41 139 canOmega[i]=a;
maxnagazumi 0:187c4874bf41 140 }
maxnagazumi 0:187c4874bf41 141 break;
maxnagazumi 0:187c4874bf41 142 case 11:
maxnagazumi 0:187c4874bf41 143 for(int i=0; i<4; i++) {
maxnagazumi 0:187c4874bf41 144 canOmega[i]=-a;
maxnagazumi 0:187c4874bf41 145 }
maxnagazumi 0:187c4874bf41 146 break;
maxnagazumi 0:187c4874bf41 147 case 12:
maxnagazumi 0:187c4874bf41 148 wait(0.2);
maxnagazumi 0:187c4874bf41 149 if(count==1) {
maxnagazumi 0:187c4874bf41 150 a=0.1;
maxnagazumi 0:187c4874bf41 151 count=0;
maxnagazumi 0:187c4874bf41 152 } else {
maxnagazumi 0:187c4874bf41 153 a=0.05;
maxnagazumi 0:187c4874bf41 154 count=1;
maxnagazumi 0:187c4874bf41 155 }
maxnagazumi 0:187c4874bf41 156 ledturn();
maxnagazumi 0:187c4874bf41 157 break;
maxnagazumi 0:187c4874bf41 158
maxnagazumi 0:187c4874bf41 159 }
maxnagazumi 0:187c4874bf41 160 for(int i=0; i<4; i++) {
maxnagazumi 0:187c4874bf41 161 if(canOmega[i]>0) {
maxnagazumi 0:187c4874bf41 162 canOmega[i]=canOmega[i]*0.8;
maxnagazumi 0:187c4874bf41 163 }
maxnagazumi 0:187c4874bf41 164 motor[i].turn(canOmega[i]);
maxnagazumi 0:187c4874bf41 165 }
maxnagazumi 0:187c4874bf41 166 }
maxnagazumi 0:187c4874bf41 167
maxnagazumi 0:187c4874bf41 168 //ticker に入れる関数
maxnagazumi 0:187c4874bf41 169 Ticker canTicker;
maxnagazumi 0:187c4874bf41 170
maxnagazumi 0:187c4874bf41 171 CAN_ticker canx;
maxnagazumi 0:187c4874bf41 172 void ticker_CanRead()
maxnagazumi 0:187c4874bf41 173 {
maxnagazumi 0:187c4874bf41 174 canx.canmsg_read();
maxnagazumi 0:187c4874bf41 175 }
maxnagazumi 0:187c4874bf41 176
maxnagazumi 0:187c4874bf41 177 int main()
maxnagazumi 0:187c4874bf41 178 {
maxnagazumi 0:187c4874bf41 179 can1.frequency(1000000);
maxnagazumi 0:187c4874bf41 180 printf("start\r\n");
maxnagazumi 0:187c4874bf41 181 motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
maxnagazumi 0:187c4874bf41 182 motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
maxnagazumi 0:187c4874bf41 183 motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
maxnagazumi 0:187c4874bf41 184 motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);
maxnagazumi 0:187c4874bf41 185
maxnagazumi 0:187c4874bf41 186 motor[0].setDutyLimit(0.4);
maxnagazumi 0:187c4874bf41 187 motor[1].setDutyLimit(0.4);
maxnagazumi 0:187c4874bf41 188 motor[2].setDutyLimit(0.4);
maxnagazumi 0:187c4874bf41 189 motor[3].setDutyLimit(0.4);
maxnagazumi 0:187c4874bf41 190
maxnagazumi 0:187c4874bf41 191 motor[0].setPDparam( 0.01790, 0.00560);
maxnagazumi 0:187c4874bf41 192 motor[1].setPDparam( 0.01705, 0.00620);
maxnagazumi 0:187c4874bf41 193 motor[2].setPDparam( 0.01790, 0.00620);
maxnagazumi 0:187c4874bf41 194 motor[3].setPDparam( 0.01680, 0.00560);
maxnagazumi 0:187c4874bf41 195
maxnagazumi 0:187c4874bf41 196 int canX=20;//can変数
maxnagazumi 0:187c4874bf41 197 canTicker.attach(&ticker_CanRead,0.05);//can読み込み
maxnagazumi 0:187c4874bf41 198 while(1) {
maxnagazumi 0:187c4874bf41 199 printf("waiting\r\n");
maxnagazumi 0:187c4874bf41 200 canX=canx.get_xCAN();//０で手\動化
maxnagazumi 0:187c4874bf41 201 if(canX==13) {//手動化
maxnagazumi 0:187c4874bf41 202 wait(1);
maxnagazumi 0:187c4874bf41 203 canX=5;
maxnagazumi 0:187c4874bf41 204 while(1) {
maxnagazumi 0:187c4874bf41 205 printf("go\r\n");
maxnagazumi 0:187c4874bf41 206 calOmega_CAN(canX);
maxnagazumi 0:187c4874bf41 207 if(canX==13) {
maxnagazumi 0:187c4874bf41 208 wait(0.5);
maxnagazumi 0:187c4874bf41 209 break;
maxnagazumi 0:187c4874bf41 210 }
maxnagazumi 0:187c4874bf41 211 if(canX==14) {
maxnagazumi 0:187c4874bf41 212 printf("reset\r\n");
maxnagazumi 0:187c4874bf41 213 NVIC_SystemReset();
maxnagazumi 0:187c4874bf41 214 }
maxnagazumi 0:187c4874bf41 215 canX=canx.get_xCAN();
maxnagazumi 0:187c4874bf41 216 }
maxnagazumi 0:187c4874bf41 217 }
maxnagazumi 0:187c4874bf41 218 }
maxnagazumi 0:187c4874bf41 219 }