Hajime Muraki / Mbed 2 deprecated chronograph

main

Dependencies:   TextLCD mbed PID

main.cpp@0:e6d14fec4954, 2014-02-25 (annotated)

Committer:
com3
Date:
Tue Feb 25 03:06:52 2014 +0000
Revision:
0:e6d14fec4954
Child:
1:fb4277ce4d93
Chronograph

Who changed what in which revision?

UserRevisionLine numberNew contents of line
com3 0:e6d14fec4954 1 #include "mbed.h"
com3 0:e6d14fec4954 2 #include "HMC6352.h"
com3 0:e6d14fec4954 3 #include "TextLCD.h"
com3 0:e6d14fec4954 4 #include "common.h"
com3 0:e6d14fec4954 5 #include <math.h>
com3 0:e6d14fec4954 6 #include <sstream>
com3 0:e6d14fec4954 7
com3 0:e6d14fec4954 8 #define LINE_P 70
com3 0:e6d14fec4954 9 #define R 1.0
com3 0:e6d14fec4954 10
com3 0:e6d14fec4954 11 DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
com3 0:e6d14fec4954 12 Serial motor(p9,p10);
com3 0:e6d14fec4954 13 Serial sensor(p13,p14);
com3 0:e6d14fec4954 14 Serial pc(USBTX, USBRX);
com3 0:e6d14fec4954 15 TextLCD lcd(p26, p25, p24, p23, p22, p21);
com3 0:e6d14fec4954 16 //AnalogIn adcIn[4] = {p17, p18, p19, p20};
com3 0:e6d14fec4954 17 //HMC6352 dcompass(p9,p10);
com3 0:e6d14fec4954 18
com3 0:e6d14fec4954 19 extern string StringFIN;
com3 0:e6d14fec4954 20 extern void array(int,int,int,int);
com3 0:e6d14fec4954 21 extern void micon_rx(void);
com3 0:e6d14fec4954 22
com3 0:e6d14fec4954 23 //uint16_t analogHex[4] = {0};
com3 0:e6d14fec4954 24 int speed[4] = {0};
com3 0:e6d14fec4954 25 uint8_t ping[4] = {0};
com3 0:e6d14fec4954 26 uint8_t line[4] = {0};
com3 0:e6d14fec4954 27 uint8_t ir_max = 0, ir_num = 0;
com3 0:e6d14fec4954 28 int compass = 0;
com3 0:e6d14fec4954 29 int x = 0, y = 0, s = 0, i = 0, line_on = 0;
com3 0:e6d14fec4954 30 int compassdef = 0, data = 0;
com3 0:e6d14fec4954 31
com3 0:e6d14fec4954 32
com3 0:e6d14fec4954 33 void move(int vx, int vy, int vs){
com3 0:e6d14fec4954 34 double pwm[4] = {0};
com3 0:e6d14fec4954 35
com3 0:e6d14fec4954 36 pwm[0] = (double)((vx) + vs);
com3 0:e6d14fec4954 37 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
com3 0:e6d14fec4954 38 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
com3 0:e6d14fec4954 39 pwm[3] = 0;
com3 0:e6d14fec4954 40
com3 0:e6d14fec4954 41 for(i = 0; i < 4; i++){
com3 0:e6d14fec4954 42 if(pwm[i] > 100){
com3 0:e6d14fec4954 43 pwm[i] = 100;
com3 0:e6d14fec4954 44 } else if(pwm[i] < -100){
com3 0:e6d14fec4954 45 pwm[i] = -100;
com3 0:e6d14fec4954 46 }
com3 0:e6d14fec4954 47 speed[i] = pwm[i];
com3 0:e6d14fec4954 48 }
com3 0:e6d14fec4954 49 }
com3 0:e6d14fec4954 50
com3 0:e6d14fec4954 51 //通信（モータ用）
com3 0:e6d14fec4954 52 void tx_motor(){
com3 0:e6d14fec4954 53 array(speed[0],speed[1],speed[3],speed[2]);
com3 0:e6d14fec4954 54 motor.printf("%s",StringFIN.c_str());
com3 0:e6d14fec4954 55 }
com3 0:e6d14fec4954 56
com3 0:e6d14fec4954 57 //ライン判断
com3 0:e6d14fec4954 58 void line_state(){
com3 0:e6d14fec4954 59 if(line[0]){
com3 0:e6d14fec4954 60 y = -LINE_P;
com3 0:e6d14fec4954 61 }
com3 0:e6d14fec4954 62 if(line[1]){
com3 0:e6d14fec4954 63 x = LINE_P;
com3 0:e6d14fec4954 64 }
com3 0:e6d14fec4954 65 if(line[2]){
com3 0:e6d14fec4954 66 y = LINE_P;
com3 0:e6d14fec4954 67 }
com3 0:e6d14fec4954 68 if(line[3]){
com3 0:e6d14fec4954 69 x = -LINE_P;
com3 0:e6d14fec4954 70 }
com3 0:e6d14fec4954 71 }
com3 0:e6d14fec4954 72
com3 0:e6d14fec4954 73 int main() {
com3 0:e6d14fec4954 74
com3 0:e6d14fec4954 75 //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
com3 0:e6d14fec4954 76
com3 0:e6d14fec4954 77 wait(1);
com3 0:e6d14fec4954 78
com3 0:e6d14fec4954 79 motor.baud(115200); //ボーレート設定
com3 0:e6d14fec4954 80 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
com3 0:e6d14fec4954 81 motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
com3 0:e6d14fec4954 82 sensor.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
com3 0:e6d14fec4954 83 //compassdef = (compass / 10); //コンパス初期値保存
com3 0:e6d14fec4954 84 //compassdef = (dcompass.sample() / 10);
com3 0:e6d14fec4954 85
com3 0:e6d14fec4954 86
com3 0:e6d14fec4954 87 wait(1);
com3 0:e6d14fec4954 88
com3 0:e6d14fec4954 89 //while(1);
com3 0:e6d14fec4954 90
com3 0:e6d14fec4954 91 y = 20;
com3 0:e6d14fec4954 92
com3 0:e6d14fec4954 93 while(1){
com3 0:e6d14fec4954 94 s = (compass - 180) / 5;
com3 0:e6d14fec4954 95 //pc.printf("%d\n", s);
com3 0:e6d14fec4954 96
com3 0:e6d14fec4954 97 move(x,y,s);
com3 0:e6d14fec4954 98 //wait(0.1);
com3 0:e6d14fec4954 99 }
com3 0:e6d14fec4954 100 /*
com3 0:e6d14fec4954 101 while(1){
com3 0:e6d14fec4954 102 i = 3;
com3 0:e6d14fec4954 103 analogHex[i] = adcIn[i].read_u16();
com3 0:e6d14fec4954 104 if(analogHex[i] > 10000){
com3 0:e6d14fec4954 105 myled[i] = 1;
com3 0:e6d14fec4954 106 line[i] = 1;
com3 0:e6d14fec4954 107 line_on = 1;
com3 0:e6d14fec4954 108 } else {
com3 0:e6d14fec4954 109 myled[i] = 0;
com3 0:e6d14fec4954 110 line[i] = 0;
com3 0:e6d14fec4954 111 line_on = 0;
com3 0:e6d14fec4954 112 }
com3 0:e6d14fec4954 113
com3 0:e6d14fec4954 114 x = 30;
com3 0:e6d14fec4954 115 y = 0;
com3 0:e6d14fec4954 116
com3 0:e6d14fec4954 117 if(line[1]){
com3 0:e6d14fec4954 118 x = LINE_P;
com3 0:e6d14fec4954 119 } else if(line[3]){
com3 0:e6d14fec4954 120 x = -LINE_P;
com3 0:e6d14fec4954 121 }
com3 0:e6d14fec4954 122
com3 0:e6d14fec4954 123 if(line[0]){
com3 0:e6d14fec4954 124 y = -LINE_P;
com3 0:e6d14fec4954 125 } else if(line[2]){
com3 0:e6d14fec4954 126 y = LINE_P;
com3 0:e6d14fec4954 127 }
com3 0:e6d14fec4954 128 move(x,y,s);
com3 0:e6d14fec4954 129
com3 0:e6d14fec4954 130 if(line_on)wait(0.3);
com3 0:e6d14fec4954 131 }
com3 0:e6d14fec4954 132 */
com3 0:e6d14fec4954 133 /*
com3 0:e6d14fec4954 134 while(1){
com3 0:e6d14fec4954 135 for(i = 0; i < 4; i++){
com3 0:e6d14fec4954 136 analogHex[i] = adcIn[i].read_u16();
com3 0:e6d14fec4954 137 if(analogHex[i] > 10000){
com3 0:e6d14fec4954 138 myled[i] = 1;
com3 0:e6d14fec4954 139 line[i] = 1;
com3 0:e6d14fec4954 140 } else {
com3 0:e6d14fec4954 141 myled[i] = 0;
com3 0:e6d14fec4954 142 line[i] = 0;
com3 0:e6d14fec4954 143 }
com3 0:e6d14fec4954 144 //wait(0.1);
com3 0:e6d14fec4954 145 }
com3 0:e6d14fec4954 146
com3 0:e6d14fec4954 147 if(line[0]){
com3 0:e6d14fec4954 148 x = -30;
com3 0:e6d14fec4954 149 } else if(line[2]){
com3 0:e6d14fec4954 150 x = 30;
com3 0:e6d14fec4954 151 }
com3 0:e6d14fec4954 152
com3 0:e6d14fec4954 153 if(line[1]){
com3 0:e6d14fec4954 154 y = 30;
com3 0:e6d14fec4954 155 } else if(line[3]){
com3 0:e6d14fec4954 156 y = -30;
com3 0:e6d14fec4954 157 }
com3 0:e6d14fec4954 158
com3 0:e6d14fec4954 159 move(x,y);
com3 0:e6d14fec4954 160 x = 0;
com3 0:e6d14fec4954 161 y = 0;
com3 0:e6d14fec4954 162 //pc.printf("%05d %05d %05d %05d\n", analogHex[0], analogHex[1], analogHex[2], analogHex[3]);
com3 0:e6d14fec4954 163 }
com3 0:e6d14fec4954 164 */
com3 0:e6d14fec4954 165 }