Kiko Ishimoto
/
sotuken_mother_2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: ds3_si mbed omuni solenoid
Fork of
2017_Robocon_mother
by 
gaku takasawa
Diff: main.cpp
- Revision:
- 9:8fd838e5a0a1
- Parent:
- 8:030080071a4a
--- a/main.cpp Wed Oct 25 08:53:38 2017 +0000
+++ b/main.cpp Fri Mar 02 02:56:37 2018 +0000
@@ -1,25 +1,21 @@
#include "mbed.h"
#include "omuni.h"
-#include "solenoid.h"
-
-#define DEBUG 0
+#include "ds3_si.h"
+#define DEBUG 1
#define CON_OFFSET 15
I2C i2c(p28, p27);
omuni omu(&i2c, 0x10, 0x14, 0x16);
-solenoid sol(&i2c, 0x20);
-Serial con(p9, p10);
-Serial master(p13,p14);
Serial pc(USBTX, USBRX);
+ds3_si con(p9,p10,2400);
-BusOut serialsel(p19,p20,LED1,LED2);
+BusOut serialsel(LED1,LED2,LED3,LED4);
Timer serialtimer;
//Ticker readtimer;
-char ConData[2][12];
-char offset[4];
+char ConData[5];
/*
void mbedreset()
@@ -30,97 +26,108 @@
void GetData()
{
- //readtimer.attach(&mbedreset, 1);
+ __disable_irq();
+ NVIC_ClearPendingIRQ(UART0_IRQn);
+ NVIC_ClearPendingIRQ(UART1_IRQn);
+ NVIC_ClearPendingIRQ(UART2_IRQn);
+ NVIC_ClearPendingIRQ(UART3_IRQn);
static bool main_flag = 1 ;
- if ( con.getc() == 'H' ) {
- ConData[0][0] = 'H';
- for (int i = 1; i < 12; i++)
- {
- char t = (char)con.getc();
- ConData[0][i] = t;
- }
- for (int i = 0; i < 12; i++)
- {
- char t = (char)con.getc();
- ConData[1][i] = t;
- }
- for(int i = 0; i < 12; i++)
+ if ( pc.getc() == 'H' ) {
+ ConData[0] = 'H';
+ for (int i = 1; i < 5; i++)
{
- master.putc(ConData[0][i]);
- }
- for(int i = 0; i < 12; i++)
- {
- master.putc(ConData[1][i]);
+ char t = (char)pc.getc();
+ ConData[i] = t;
}
if(main_flag)
{
- /*
-
- offset[0] = ConData[0][1];
- offset[1] = ConData[0][2];
- offset[2] = ConData[1][1];
- offset[3] = ConData[1][2];
- */
serialtimer.start();
main_flag = 0;
}
- //ConData[0][1] -= offset[0];
- //ConData[0][2] -= offset[1];
- //ConData[1][1] -= offset[2];
- //ConData[1][2] -= offset[3];
-
- if( (char)255 - CON_OFFSET < ConData[1][1] || ConData[1][1] < CON_OFFSET)
- ConData[1][1] = 0;
serialtimer.reset();
}
- //readtimer.detach();
+ __enable_irq();
+}
+#define deadZone 2
+bool triggerAnalog(){
+ return abs(con.analogstate(L3x)) > deadZone || abs(con.analogstate(L3y)) > deadZone
+ || abs(con.analogstate(R3y)) > deadZone || abs(con.analogstate(R3y)) > deadZone;
}
-
+char MotorReset[] = {'H', 0, 0, 0, 0};
int main() {
serialsel = 0x05;
- con.baud(115200);
- master.baud(115200);
pc.baud(115200);
- con.attach(&GetData,Serial::RxIrq);
+ pc.attach(&GetData,Serial::RxIrq);
serialtimer.stop();
serialtimer.reset();
while(1){
- char MotorData[] = {'H', ConData[0][1], ConData[0][2], ConData[1][1], ConData[1][2]};
- omu.out(MotorData);
-
- char soldata = ((ConData[0][3] << 2) + ConData[1][3]) << 4;
- sol = soldata;
+ wait(0.01);
+ int8_t MotorData[] = {'H', ConData[1], ConData[2], ConData[3], ConData[4]};
+ MotorData[3] = con.analogstate(L3x);
+ MotorData[4] = con.analogstate(L3y);
+ MotorData[1] = con.analogstate(R3x);
+ MotorData[2] = con.analogstate(R3y);
if(serialtimer.read_ms() > 500)
{
- serialsel = ~serialsel;
- serialtimer.reset();
- char MotorReset[] = {'H', 0, 0, 0, 0};
+ serialsel = 1;
omu.out(MotorReset);
+ }else{
+ if(triggerAnalog() == true){
+ if(ConData[1] == 'S' && ConData[2] == 'T'&& ConData[3] == 'O' && ConData[4] == 'P'){
+ MotorData[4] = MotorData[4] > 0 ? 0 : MotorData[4];
+ serialsel = 0xF;
+ }else if(ConData[1] == 'O' && ConData[2] == 'N'&& ConData[3] == 'E'){
+ MotorData[4] = MotorData[4] > 60 ? 60 : MotorData[4];
+ if(MotorData[4] > 0)
+ if(ConData[4] == 'R'){
+ MotorData[1] = 30;
+ MotorData[2] = 0;
+ serialsel = 0x2;
+ }
+ else if(ConData[4] == 'L') {
+ MotorData[1] = -30;
+ MotorData[2] = 0;
+ serialsel = 0x4;
+ }
+ }else if(ConData[1] == 'E' && ConData[2] == 'S'&& ConData[3] == 'T'){
+ MotorData[4] = MotorData[4] > 40 ? 40 : MotorData[4];
+ if(MotorData[4] > 0)
+ if(ConData[4] == 'R'){
+ MotorData[1] = 30;
+ MotorData[2] = 0;
+ serialsel = 0x2;
+ }else if(ConData[4] == 'L') {
+ MotorData[1] = -30;
+ MotorData[2] = 0;
+ serialsel = 0x4;
+ }else if(ConData[4] == 'G'){
+ MotorData[3] = 0;
+ MotorData[2] = 0;
+ }
+ }else if(ConData[1] == 'F' && ConData[2] == 'R'&& ConData[3] == 'E' && ConData[4] == 'E'){
+ serialsel = 0x0;
+ }
+ omu.out((char*)MotorData);
+ }else{
+ serialsel = 1;
+ omu.out(MotorReset);
+ }
+ /*
+ #if DEBUG
+ for(int i = 0; i < 5; i++)
+ {
+ pc.printf("%d",MotorData[i]);
+ }
+ pc.printf("\n");
+ #endif*/
+
}
- #if DEBUG
- for(int i = 0; i < 12; i++)
- {
- pc.printf("%3d ",ConData[0][i]);
- }
- for(int i = 0; i < 12; i++)
- {
- pc.printf("%3d ",ConData[1][i]);
- }
- pc.printf("\n\r");
-
- for(int i = 0; i < 4; i++)
- {
- pc.printf("%3d ",offset[i]);
- }
- printf("\n");
-
- printf("%d\n",soldata);
- #endif
+
}
}