taiyou komazawa / Mbed 2 deprecated mbed_h28hongo1A_seiyaho-main

toyama-NCT hongo seiyaho_main

Dependencies:   mbed

main.cpp@0:7f0b090897bc, 2016-12-26 (annotated)

Committer:
Komazawa_sun
Date:
Mon Dec 26 06:15:07 2016 +0000
Revision:
0:7f0b090897bc
h28?????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Komazawa_sun 0:7f0b090897bc 1 #include "mbed.h"
Komazawa_sun 0:7f0b090897bc 2
Komazawa_sun 0:7f0b090897bc 3 #define PWMMAX 0.33
Komazawa_sun 0:7f0b090897bc 4 #define stMAX 1023
Komazawa_sun 0:7f0b090897bc 5 #define stMIN 0
Komazawa_sun 0:7f0b090897bc 6 #define daMAX 255
Komazawa_sun 0:7f0b090897bc 7 #define SERVO_MIN_ANG 1020 //us 0.0
Komazawa_sun 0:7f0b090897bc 8
Komazawa_sun 0:7f0b090897bc 9 Serial comUSB(USBTX,USBRX);
Komazawa_sun 0:7f0b090897bc 10 Serial com1(p9,p10);
Komazawa_sun 0:7f0b090897bc 11 //Serial com1(p13, p14);
Komazawa_sun 0:7f0b090897bc 12
Komazawa_sun 0:7f0b090897bc 13 DigitalIn angMAX(p18);
Komazawa_sun 0:7f0b090897bc 14 DigitalIn angMIN(p19);
Komazawa_sun 0:7f0b090897bc 15 DigitalOut bufcheck(LED1);
Komazawa_sun 0:7f0b090897bc 16 DigitalOut dirR(p28);
Komazawa_sun 0:7f0b090897bc 17 DigitalOut dirL(p29);
Komazawa_sun 0:7f0b090897bc 18 DigitalOut dirA(p20);
Komazawa_sun 0:7f0b090897bc 19 DigitalOut shoter(p11);
Komazawa_sun 0:7f0b090897bc 20 DigitalOut loader(p12);
Komazawa_sun 0:7f0b090897bc 21 PwmOut servo(p24);
Komazawa_sun 0:7f0b090897bc 22 PwmOut pwrR(p26);
Komazawa_sun 0:7f0b090897bc 23 PwmOut pwrL(p25);
Komazawa_sun 0:7f0b090897bc 24 PwmOut pwrA(p21);
Komazawa_sun 0:7f0b090897bc 25
Komazawa_sun 0:7f0b090897bc 26 const float angpwr = 0.18;
Komazawa_sun 0:7f0b090897bc 27 int buf_count = -1;
Komazawa_sun 0:7f0b090897bc 28
Komazawa_sun 0:7f0b090897bc 29 union Get_data{
Komazawa_sun 0:7f0b090897bc 30 char alldata[6];
Komazawa_sun 0:7f0b090897bc 31 struct{
Komazawa_sun 0:7f0b090897bc 32 char header :8;
Komazawa_sun 0:7f0b090897bc 33 unsigned int x :8;
Komazawa_sun 0:7f0b090897bc 34 unsigned int y :8;
Komazawa_sun 0:7f0b090897bc 35 unsigned int t :8;
Komazawa_sun 0:7f0b090897bc 36 int au :1;
Komazawa_sun 0:7f0b090897bc 37 int ad :1;
Komazawa_sun 0:7f0b090897bc 38 int h :1;
Komazawa_sun 0:7f0b090897bc 39 int set :1;
Komazawa_sun 0:7f0b090897bc 40 int s :1;
Komazawa_sun 0:7f0b090897bc 41 int vx :1;
Komazawa_sun 0:7f0b090897bc 42 int vy :1;
Komazawa_sun 0:7f0b090897bc 43 int vt :1;
Komazawa_sun 0:7f0b090897bc 44 }data;
Komazawa_sun 0:7f0b090897bc 45 }buf_data;
Komazawa_sun 0:7f0b090897bc 46
Komazawa_sun 0:7f0b090897bc 47 union union_data{
Komazawa_sun 0:7f0b090897bc 48 char alldata[9];
Komazawa_sun 0:7f0b090897bc 49 struct{
Komazawa_sun 0:7f0b090897bc 50 char header :8;
Komazawa_sun 0:7f0b090897bc 51 signed int x :16;
Komazawa_sun 0:7f0b090897bc 52 signed int y :16;
Komazawa_sun 0:7f0b090897bc 53 signed int t :16;
Komazawa_sun 0:7f0b090897bc 54 int au :1;
Komazawa_sun 0:7f0b090897bc 55 int ad :1;
Komazawa_sun 0:7f0b090897bc 56 int h :1;
Komazawa_sun 0:7f0b090897bc 57 int set :1;
Komazawa_sun 0:7f0b090897bc 58 int s :1;
Komazawa_sun 0:7f0b090897bc 59 }data;
Komazawa_sun 0:7f0b090897bc 60 }act_data;
Komazawa_sun 0:7f0b090897bc 61
Komazawa_sun 0:7f0b090897bc 62
Komazawa_sun 0:7f0b090897bc 63
Komazawa_sun 0:7f0b090897bc 64 void Serialsetup(){
Komazawa_sun 0:7f0b090897bc 65 comUSB.baud(9600);
Komazawa_sun 0:7f0b090897bc 66 com1.baud(115200);
Komazawa_sun 0:7f0b090897bc 67 }
Komazawa_sun 0:7f0b090897bc 68
Komazawa_sun 0:7f0b090897bc 69 void interface(){
Komazawa_sun 0:7f0b090897bc 70 char data;
Komazawa_sun 0:7f0b090897bc 71 bufcheck = 1;
Komazawa_sun 0:7f0b090897bc 72 while(com1.readable()){
Komazawa_sun 0:7f0b090897bc 73 data = com1.getc();
Komazawa_sun 0:7f0b090897bc 74 bufcheck = 1;
Komazawa_sun 0:7f0b090897bc 75 //comUSB.printf("%d\r\n",data);
Komazawa_sun 0:7f0b090897bc 76 if(data == 0xFE){
Komazawa_sun 0:7f0b090897bc 77 buf_count = 0;
Komazawa_sun 0:7f0b090897bc 78
Komazawa_sun 0:7f0b090897bc 79 }
Komazawa_sun 0:7f0b090897bc 80 if(buf_count < 6 && buf_count != -1){
Komazawa_sun 0:7f0b090897bc 81 buf_data.alldata[buf_count] = data;
Komazawa_sun 0:7f0b090897bc 82 buf_count++;
Komazawa_sun 0:7f0b090897bc 83 }
Komazawa_sun 0:7f0b090897bc 84
Komazawa_sun 0:7f0b090897bc 85 }
Komazawa_sun 0:7f0b090897bc 86
Komazawa_sun 0:7f0b090897bc 87 if(buf_count == 6 && buf_data.alldata[0] == 0xFE){
Komazawa_sun 0:7f0b090897bc 88 if(buf_data.data.vx == 0){
Komazawa_sun 0:7f0b090897bc 89 act_data.data.x = (signed int)buf_data.data.x * -1;
Komazawa_sun 0:7f0b090897bc 90 }else{
Komazawa_sun 0:7f0b090897bc 91 act_data.data.x = buf_data.data.x;
Komazawa_sun 0:7f0b090897bc 92 }
Komazawa_sun 0:7f0b090897bc 93
Komazawa_sun 0:7f0b090897bc 94 if(buf_data.data.vy == 0){
Komazawa_sun 0:7f0b090897bc 95 act_data.data.y = (signed int)buf_data.data.y * -1;
Komazawa_sun 0:7f0b090897bc 96 }else{
Komazawa_sun 0:7f0b090897bc 97 act_data.data.y = buf_data.data.y;
Komazawa_sun 0:7f0b090897bc 98 }
Komazawa_sun 0:7f0b090897bc 99
Komazawa_sun 0:7f0b090897bc 100 if(buf_data.data.vt == 0){
Komazawa_sun 0:7f0b090897bc 101 act_data.data.t = (signed int)buf_data.data.t * -1;
Komazawa_sun 0:7f0b090897bc 102 }else{
Komazawa_sun 0:7f0b090897bc 103 act_data.data.t = buf_data.data.t;
Komazawa_sun 0:7f0b090897bc 104 }
Komazawa_sun 0:7f0b090897bc 105 act_data.data.au = buf_data.data.au;
Komazawa_sun 0:7f0b090897bc 106 act_data.data.ad = buf_data.data.ad;
Komazawa_sun 0:7f0b090897bc 107 act_data.data.h = buf_data.data.h;
Komazawa_sun 0:7f0b090897bc 108 act_data.data.set = buf_data.data.set;
Komazawa_sun 0:7f0b090897bc 109 act_data.data.s = buf_data.data.s;
Komazawa_sun 0:7f0b090897bc 110 }
Komazawa_sun 0:7f0b090897bc 111
Komazawa_sun 0:7f0b090897bc 112
Komazawa_sun 0:7f0b090897bc 113
Komazawa_sun 0:7f0b090897bc 114 }
Komazawa_sun 0:7f0b090897bc 115
Komazawa_sun 0:7f0b090897bc 116
Komazawa_sun 0:7f0b090897bc 117 void drive_mech(){
Komazawa_sun 0:7f0b090897bc 118 float power,lduty,turn;
Komazawa_sun 0:7f0b090897bc 119 power = act_data.data.y * (PWMMAX / daMAX);
Komazawa_sun 0:7f0b090897bc 120 lduty = act_data.data.x * (PWMMAX / daMAX);
Komazawa_sun 0:7f0b090897bc 121 turn = act_data.data.t * (PWMMAX / daMAX);
Komazawa_sun 0:7f0b090897bc 122 if(power == 0.00){
Komazawa_sun 0:7f0b090897bc 123 if(turn < 0){
Komazawa_sun 0:7f0b090897bc 124 dirR = 1;
Komazawa_sun 0:7f0b090897bc 125 dirL = 1;
Komazawa_sun 0:7f0b090897bc 126 pwrR = turn * -1;
Komazawa_sun 0:7f0b090897bc 127 pwrL = turn * -1;
Komazawa_sun 0:7f0b090897bc 128 }else if(turn >= 0){
Komazawa_sun 0:7f0b090897bc 129 dirR = 0;
Komazawa_sun 0:7f0b090897bc 130 dirL = 0;
Komazawa_sun 0:7f0b090897bc 131 pwrR = turn;
Komazawa_sun 0:7f0b090897bc 132 pwrL = turn;
Komazawa_sun 0:7f0b090897bc 133 }
Komazawa_sun 0:7f0b090897bc 134 }else{
Komazawa_sun 0:7f0b090897bc 135 if(power <= 0){
Komazawa_sun 0:7f0b090897bc 136 power = power * -1;
Komazawa_sun 0:7f0b090897bc 137 dirR = 1;
Komazawa_sun 0:7f0b090897bc 138 dirL = 0;
Komazawa_sun 0:7f0b090897bc 139 }else if (power > 0){
Komazawa_sun 0:7f0b090897bc 140 dirR = 0;
Komazawa_sun 0:7f0b090897bc 141 dirL = 1;
Komazawa_sun 0:7f0b090897bc 142 }
Komazawa_sun 0:7f0b090897bc 143
Komazawa_sun 0:7f0b090897bc 144 if(lduty <= 0){
Komazawa_sun 0:7f0b090897bc 145 pwrR = power;
Komazawa_sun 0:7f0b090897bc 146 pwrL = power - (lduty * -1) / 2;
Komazawa_sun 0:7f0b090897bc 147 }else if(lduty > 0){
Komazawa_sun 0:7f0b090897bc 148 pwrR = power - lduty / 2;
Komazawa_sun 0:7f0b090897bc 149 pwrL = power;
Komazawa_sun 0:7f0b090897bc 150 }
Komazawa_sun 0:7f0b090897bc 151 //comUSB.printf("p %f, l %f,t %f\r\n",power,lduty,turn);
Komazawa_sun 0:7f0b090897bc 152 }
Komazawa_sun 0:7f0b090897bc 153 }
Komazawa_sun 0:7f0b090897bc 154
Komazawa_sun 0:7f0b090897bc 155
Komazawa_sun 0:7f0b090897bc 156 void seiyaho(){
Komazawa_sun 0:7f0b090897bc 157 if(act_data.data.au == 1){
Komazawa_sun 0:7f0b090897bc 158 if(angMAX != 1){
Komazawa_sun 0:7f0b090897bc 159 dirA = 1;
Komazawa_sun 0:7f0b090897bc 160 pwrA = angpwr;
Komazawa_sun 0:7f0b090897bc 161 }else if(angMAX == 1){
Komazawa_sun 0:7f0b090897bc 162 pwrA = 0.00;
Komazawa_sun 0:7f0b090897bc 163 }
Komazawa_sun 0:7f0b090897bc 164 }else if(act_data.data.ad == 1){
Komazawa_sun 0:7f0b090897bc 165 if(angMIN != 1){
Komazawa_sun 0:7f0b090897bc 166 dirA = 0;
Komazawa_sun 0:7f0b090897bc 167 pwrA = angpwr;
Komazawa_sun 0:7f0b090897bc 168 }else if(angMIN == 1){
Komazawa_sun 0:7f0b090897bc 169 pwrA = 0.00;
Komazawa_sun 0:7f0b090897bc 170 }
Komazawa_sun 0:7f0b090897bc 171 }else if(act_data.data.au != 1 && act_data.data.ad != 1) {
Komazawa_sun 0:7f0b090897bc 172 pwrA = 0.00;
Komazawa_sun 0:7f0b090897bc 173 }
Komazawa_sun 0:7f0b090897bc 174
Komazawa_sun 0:7f0b090897bc 175 }
Komazawa_sun 0:7f0b090897bc 176
Komazawa_sun 0:7f0b090897bc 177
Komazawa_sun 0:7f0b090897bc 178 void hold(){
Komazawa_sun 0:7f0b090897bc 179 if(act_data.data.h == 1 && act_data.data.s != 1 && act_data.data.s != 1){
Komazawa_sun 0:7f0b090897bc 180 servo.pulsewidth_us(SERVO_MIN_ANG + 900);
Komazawa_sun 0:7f0b090897bc 181 }else if(act_data.data.h != 1 && act_data.data.set != 1){
Komazawa_sun 0:7f0b090897bc 182 servo.pulsewidth_us(SERVO_MIN_ANG + 500 * (0 / 90));
Komazawa_sun 0:7f0b090897bc 183 }else if(act_data.data.set == 1 && act_data.data.s != 1 && act_data.data.s != 1){
Komazawa_sun 0:7f0b090897bc 184 servo.pulsewidth_us(SERVO_MIN_ANG + 1280);
Komazawa_sun 0:7f0b090897bc 185 }
Komazawa_sun 0:7f0b090897bc 186 }
Komazawa_sun 0:7f0b090897bc 187
Komazawa_sun 0:7f0b090897bc 188
Komazawa_sun 0:7f0b090897bc 189 void fire(){
Komazawa_sun 0:7f0b090897bc 190 if(act_data.data.s == 1 && act_data.data.h != 1 && act_data.data.set != 1){
Komazawa_sun 0:7f0b090897bc 191 loader = 0;
Komazawa_sun 0:7f0b090897bc 192 shoter = 1;
Komazawa_sun 0:7f0b090897bc 193 }else if(act_data.data.s == 0 && act_data.data.h != 1 && act_data.data.set != 1){
Komazawa_sun 0:7f0b090897bc 194 loader = 1;
Komazawa_sun 0:7f0b090897bc 195 shoter = 0;
Komazawa_sun 0:7f0b090897bc 196 }
Komazawa_sun 0:7f0b090897bc 197 }
Komazawa_sun 0:7f0b090897bc 198
Komazawa_sun 0:7f0b090897bc 199
Komazawa_sun 0:7f0b090897bc 200 int main() {
Komazawa_sun 0:7f0b090897bc 201 Serialsetup();
Komazawa_sun 0:7f0b090897bc 202 servo.period_ms(20);
Komazawa_sun 0:7f0b090897bc 203 while(1) {
Komazawa_sun 0:7f0b090897bc 204 bufcheck = 0;
Komazawa_sun 0:7f0b090897bc 205 com1.attach(interface,Serial::RxIrq);
Komazawa_sun 0:7f0b090897bc 206 fire();
Komazawa_sun 0:7f0b090897bc 207 drive_mech();
Komazawa_sun 0:7f0b090897bc 208 seiyaho();
Komazawa_sun 0:7f0b090897bc 209 hold();
Komazawa_sun 0:7f0b090897bc 210 /*comUSB.printf("x %d,y %d,t %d,u %d,d %d,h %d,%d,s ,%d %d,%d,%d\r\n",
Komazawa_sun 0:7f0b090897bc 211 act_data.data.x,act_data.data.y,act_data.data.t,
Komazawa_sun 0:7f0b090897bc 212 act_data.data.au,act_data.data.ad,act_data.data.h,act_data.data.set,
Komazawa_sun 0:7f0b090897bc 213 act_data.data.s,buf_data.data.vx,buf_data.data.vy,buf_data.data.vt);*/
Komazawa_sun 0:7f0b090897bc 214
Komazawa_sun 0:7f0b090897bc 215 }
Komazawa_sun 0:7f0b090897bc 216 }