Hajime Muraki
main
Dependencies: TextLCD mbed PID
Revision 0:e6d14fec4954, committed 2014-02-25
- Comitter:
- com3
- Date:
- Tue Feb 25 03:06:52 2014 +0000
- Child:
- 1:fb4277ce4d93
- Commit message:
- Chronograph
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Tue Feb 25 03:06:52 2014 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/simon/code/TextLCD/#44f34c09bd37
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/common.h Tue Feb 25 03:06:52 2014 +0000
@@ -0,0 +1,7 @@
+
+enum{
+ FRONT = 0,
+ LEFT,
+ BACK,
+ RIGHT,
+};
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Feb 25 03:06:52 2014 +0000
@@ -0,0 +1,165 @@
+#include "mbed.h"
+#include "HMC6352.h"
+#include "TextLCD.h"
+#include "common.h"
+#include <math.h>
+#include <sstream>
+
+#define LINE_P 70
+#define R 1.0
+
+DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
+Serial motor(p9,p10);
+Serial sensor(p13,p14);
+Serial pc(USBTX, USBRX);
+TextLCD lcd(p26, p25, p24, p23, p22, p21);
+//AnalogIn adcIn[4] = {p17, p18, p19, p20};
+//HMC6352 dcompass(p9,p10);
+
+extern string StringFIN;
+extern void array(int,int,int,int);
+extern void micon_rx(void);
+
+//uint16_t analogHex[4] = {0};
+int speed[4] = {0};
+uint8_t ping[4] = {0};
+uint8_t line[4] = {0};
+uint8_t ir_max = 0, ir_num = 0;
+int compass = 0;
+int x = 0, y = 0, s = 0, i = 0, line_on = 0;
+int compassdef = 0, data = 0;
+
+
+void move(int vx, int vy, int vs){
+ double pwm[4] = {0};
+
+ pwm[0] = (double)((vx) + vs);
+ pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
+ pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
+ pwm[3] = 0;
+
+ for(i = 0; i < 4; i++){
+ if(pwm[i] > 100){
+ pwm[i] = 100;
+ } else if(pwm[i] < -100){
+ pwm[i] = -100;
+ }
+ speed[i] = pwm[i];
+ }
+}
+
+//通信（モータ用）
+void tx_motor(){
+ array(speed[0],speed[1],speed[3],speed[2]);
+ motor.printf("%s",StringFIN.c_str());
+}
+
+//ライン判断
+void line_state(){
+ if(line[0]){
+ y = -LINE_P;
+ }
+ if(line[1]){
+ x = LINE_P;
+ }
+ if(line[2]){
+ y = LINE_P;
+ }
+ if(line[3]){
+ x = -LINE_P;
+ }
+}
+
+int main() {
+
+ //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+
+ wait(1);
+
+ motor.baud(115200); //ボーレート設定
+ motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
+ motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
+ sensor.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
+ //compassdef = (compass / 10); //コンパス初期値保存
+ //compassdef = (dcompass.sample() / 10);
+
+
+ wait(1);
+
+ //while(1);
+
+ y = 20;
+
+ while(1){
+ s = (compass - 180) / 5;
+ //pc.printf("%d\n", s);
+
+ move(x,y,s);
+ //wait(0.1);
+ }
+/*
+ while(1){
+ i = 3;
+ analogHex[i] = adcIn[i].read_u16();
+ if(analogHex[i] > 10000){
+ myled[i] = 1;
+ line[i] = 1;
+ line_on = 1;
+ } else {
+ myled[i] = 0;
+ line[i] = 0;
+ line_on = 0;
+ }
+
+ x = 30;
+ y = 0;
+
+ if(line[1]){
+ x = LINE_P;
+ } else if(line[3]){
+ x = -LINE_P;
+ }
+
+ if(line[0]){
+ y = -LINE_P;
+ } else if(line[2]){
+ y = LINE_P;
+ }
+ move(x,y,s);
+
+ if(line_on)wait(0.3);
+ }
+ */
+ /*
+ while(1){
+ for(i = 0; i < 4; i++){
+ analogHex[i] = adcIn[i].read_u16();
+ if(analogHex[i] > 10000){
+ myled[i] = 1;
+ line[i] = 1;
+ } else {
+ myled[i] = 0;
+ line[i] = 0;
+ }
+ //wait(0.1);
+ }
+
+ if(line[0]){
+ x = -30;
+ } else if(line[2]){
+ x = 30;
+ }
+
+ if(line[1]){
+ y = 30;
+ } else if(line[3]){
+ y = -30;
+ }
+
+ move(x,y);
+ x = 0;
+ y = 0;
+ //pc.printf("%05d %05d %05d %05d\n", analogHex[0], analogHex[1], analogHex[2], analogHex[3]);
+ }
+ */
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Feb 25 03:06:52 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/usart.cpp Tue Feb 25 03:06:52 2014 +0000
@@ -0,0 +1,74 @@
+#include "mbed.h"
+#include "common.h"
+
+#define KEYCODE 0xAA
+#define TX_CHECKCODE (tx_data[1]^tx_data[2]^tx_data[3]^tx_data[4]^tx_data[5]^tx_data[6]^tx_data[7]^tx_data[8]^tx_data[9]^tx_data[10])
+#define RX_CHECKCODE (rx_data[1]^rx_data[2]^rx_data[3]^rx_data[4]^rx_data[5]^rx_data[6]^rx_data[7]^rx_data[8]^rx_data[9]^rx_data[10])
+#define DATA_NUM 14
+#define CHECK (DATA_NUM - 1)
+#define ALL_IR 10
+
+
+extern Serial sensor;
+extern Serial pc;
+
+extern uint8_t ping[4];
+extern uint8_t line[4];
+extern uint8_t ir_max, ir_num;
+extern int compass;
+
+void micon_rx(){
+
+ static uint8_t rx;
+ static int rx_data[DATA_NUM];
+
+ rx_data[rx] = sensor.getc();
+
+ if(rx_data[0] == KEYCODE){
+ rx++;
+ }
+
+ if(rx >= DATA_NUM){
+ if(rx_data[CHECK] == RX_CHECKCODE){
+ compass = rx_data[11] + rx_data[12];
+ ping[FRONT] = rx_data[2];
+ ping[LEFT] = rx_data[3];
+ ping[BACK] = rx_data[4];
+ ping[RIGHT] = rx_data[5];
+ line[FRONT] = rx_data[6];
+ line[LEFT] = rx_data[7];
+ line[BACK] = rx_data[8];
+ line[RIGHT] = rx_data[9];
+ //pc.printf("%d %d %d %d %d\n", rx_data[1], rx_data[2], rx_data[3], rx_data[4], rx_data[5]);
+ pc.printf("%d %d %d\n", rx_data[11], rx_data[12], compass);
+ }
+ rx = 0;
+ }
+ //pc.printf("%d\n", rx_data[rx]);
+}
+
+void micon_tx(){
+
+ static uint8_t tx;
+ static uint8_t tx_data[DATA_NUM];
+
+ if(tx >= DATA_NUM){
+ tx_data[0] = KEYCODE;
+ tx_data[1] = KEYCODE;
+ tx_data[2] = KEYCODE;
+ tx_data[3] = KEYCODE;
+ tx_data[4] = KEYCODE;
+ tx_data[5] = KEYCODE;
+ tx_data[6] = KEYCODE;
+ tx_data[7] = KEYCODE;
+ tx_data[8] = KEYCODE;
+ tx_data[9] = KEYCODE;
+ tx_data[10] = KEYCODE;
+ tx_data[11] = TX_CHECKCODE;
+
+ tx = 0;
+ }
+
+ sensor.putc(tx_data[tx]);
+ tx++;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/wordString.cpp Tue Feb 25 03:06:52 2014 +0000
@@ -0,0 +1,71 @@
+
+#include <sstream>
+#include "mbed.h"
+
+string StringFIN;
+
+using namespace std;
+
+
+//extern Serial pc; // tx, rx
+
+string IntToString(int number)
+{
+ stringstream ss;
+ ss << number;
+ return ss.str();
+}
+
+void array(int power1,int power2,int power3,int power4)
+{
+ int input[4] = {power1,power2,power3,power4};
+ int value = 0;
+ string StringA[4] = {"0","0","0","0"};
+
+
+ string StringX = "0";
+ string StringY = "0";
+ string StringZ = "0";
+ string String0 = "0";
+
+ StringFIN = "0";
+
+ for(uint8_t i = 0 ; i < 4; i++){
+
+ value = input[i];
+
+ StringX = IntToString(i+1);
+
+ if( (value < 0) && (value >= -100) ){
+ StringY = "R";
+ value = abs(value);
+ StringZ = IntToString(value);
+ }else if( (value >= 0) && (value <= 100) ){
+ StringY = "F";
+ StringZ = IntToString(value);
+ }else{
+ value = abs(value);
+ StringY = "F";
+ StringZ = "000";
+ }
+
+ if(value < 10){
+ String0 = "00";
+ StringZ = String0 + StringZ;
+ }else if(value < 100)
+ {
+ String0 = "0";
+ StringZ = String0 + StringZ;
+ }else{
+
+ }
+
+ StringA[i] = (StringX + StringY + StringZ);
+
+ if(i == 0)StringFIN = StringA[i];
+ else StringFIN += StringA[i];
+
+ }
+
+ StringFIN += "\r\n";
+}
\ No newline at end of file