50_135_507_38_Insper
2 Motores + Joystick
Dependencies: X_NUCLEO_IHM01A1 TextLCD
Fork of
HelloWorld_IHM01A1_2Motors_mbedOS
by 
ST
Diff: main.cpp
- Revision:
- 42:a04bff02f231
- Parent:
- 41:7f91e949ca88
- Child:
- 43:d08a3f6b65b5
--- a/main.cpp Thu May 03 18:23:42 2018 +0000
+++ b/main.cpp Tue May 15 14:23:01 2018 +0000
@@ -55,7 +55,7 @@
#define STEPS 3200
/* Delay in milliseconds. */
-#define DELAY_1 2000
+#define DELAY_1 20
#define DELAY_2 6000
#define DELAY_3 8000
@@ -68,40 +68,17 @@
/* Variables -----------------------------------------------------------------*/
-InterruptIn button1(PC_1);
-InterruptIn button2(PC_0);
+//InterruptIn sensorX1(PC_9);
+//InterruptIn sensorY1(PA_14);
-/* Initialization parameters. */
-//L6474_init_t init = {
-// 160, /* Acceleration rate in pps^2. Range: (0..+inf). */
-// 160, /* Deceleration rate in pps^2. Range: (0..+inf). */
-// 4000, /* Maximum speed in pps. Range: (30..10000]. */
-// 1300, /* Minimum speed in pps. Range: [30..10000). */
-// 1500, /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
-// L6474_OCD_TH_1875mA, /* Overcurrent threshold (OCD_TH register). */
-// L6474_CONFIG_OC_SD_ENABLE, /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
-// L6474_CONFIG_EN_TQREG_TVAL_USED, /* Torque regulation method (EN_TQREG field of CONFIG register). */
-// L6474_STEP_SEL_1_2, /* Step selection (STEP_SEL field of STEP_MODE register). */
-// L6474_SYNC_SEL_1_2, /* Sync selection (SYNC_SEL field of STEP_MODE register). */
-// L6474_FAST_STEP_12us, /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
-// L6474_TOFF_FAST_8us, /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
-// 3, /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
-// 21, /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
-// L6474_CONFIG_TOFF_044us, /* Target Swicthing Period (field TOFF of CONFIG register). */
-// L6474_CONFIG_SR_320V_us, /* Slew rate (POW_SR field of CONFIG register). */
-// L6474_CONFIG_INT_16MHZ, /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
-// L6474_ALARM_EN_OVERCURRENT |
-// L6474_ALARM_EN_THERMAL_SHUTDOWN |
-// L6474_ALARM_EN_THERMAL_WARNING |
-// L6474_ALARM_EN_UNDERVOLTAGE |
-// L6474_ALARM_EN_SW_TURN_ON |
-// L6474_ALARM_EN_WRONG_NPERF_CMD /* Alarm (ALARM_EN register). */
-//};
+
Serial pc(USBTX, USBRX);
-AnalogIn eixo_X(A0);
-AnalogIn eixo_Y(A1);
+AnalogIn eixo_X(A2);
+AnalogIn eixo_Y(A3);
+AnalogIn eixo_ZU(A0);
+AnalogIn eixo_ZD(A1);
@@ -109,9 +86,14 @@
float y;
float w;
float u;
+float zu;
+float zd;
-//float a;
+int count;
+
+bool flagX1=0;
+bool flagY1=0;
unsigned int minspeed = 1300;
@@ -130,39 +112,9 @@
L6474 *motor2;
L6474 *motor3;
-void flag_irq_handler(void)
-{
- /* Set ISR flag. */
- motor3->isr_flag = TRUE;
-
- /* Get the value of the status register. */
- unsigned int status = motor3->get_status();
-
- /* Check NOTPERF_CMD flag: if set, the command received by SPI can't be performed. */
- /* This often occures when a command is sent to the L6474 while it is not in HiZ state. */
- if ((status & L6474_STATUS_NOTPERF_CMD) == L6474_STATUS_NOTPERF_CMD) {
- printf(" WARNING: \"FLAG\" interrupt triggered. Non-performable command detected when updating L6474's registers while not in HiZ state.\r\n");
- }
-
- /* Reset ISR flag. */
- motor3->isr_flag = FALSE;
-}
-
/* Main ----------------------------------------------------------------------*/
-
-void pressed_cima() {
-
- motor3->run(StepperMotor::FWD);
- printf ("Ariba! \r\n");
-
-}
-void pressed_baixo() {
-
- motor3->run(StepperMotor::BWD);
- printf ("Abajo! \r\n");
-
-}
+/*
void released_1()
{
motor3->hard_stop();
@@ -177,6 +129,28 @@
}
+void paraX1() {
+ flagX1 = 1;
+ motor2->hard_stop();
+
+ }
+
+ void continuaX1() {
+ flagX1 = 0;
+ }
+
+
+ void paraY1() {
+ flagY1 = 1;
+ motor1->hard_stop();
+
+ }
+
+ void continuaY1() {
+ flagY1 = 0;
+ }
+ */
+
int main()
{
/*----- Initialization. -----*/
@@ -202,103 +176,119 @@
- /* Attaching and enabling interrupt handlers. */
- motor3->attach_flag_irq(&flag_irq_handler);
+ /* Attaching and enabling interrupt handlers.
+ motor3->attach_flag_irq(&flag_irq_handler3);
motor3->enable_flag_irq();
+ motor1->attach_flag_irq(&flag_irq_handler1);
+ motor1->enable_flag_irq();
+ motor2->attach_flag_irq(&flag_irq_handler2);
+ motor2->enable_flag_irq();
+ */
+ //sensorX1.rise(¶X1);
+ // sensorX1.fall(&continuaX1);
- button1.rise(&pressed_cima);
- button2.rise(&pressed_baixo);
- button1.fall(&released_1);
- button2.fall(&released_2);
-
+ //sensorY1.rise(¶Y1);
+ //sensorY1.fall(&continuaY1);
+
+
while(true) {
+ count = count +1;
+ // Leitura analógica
u = eixo_X.read();
w = eixo_Y.read();
-
- /* if (button1.read()==0()){
-
- button.fall()
- printf("button1 fwd \r\n\n");
-
- }
-
- // else {
- // motor3->run(StepperMotor::BWD);
- // printf("button1 bwd \r\n\n");
-
- // }
-
- if (button1.read()==0){
-
- button1.rise()
- printf("button1 bwd \r\n\n");
+ zu = eixo_ZU.read();
+ zd = eixo_ZD.read();
+ if (count == 10000) {
+ printf("zu =%f \r\n\n", zu);
+ printf("zd =%f \r\n\n", zd);
+ count = 0;
+ }
- }*/
-
-
- if (u>0.740) {
-
- motor1->run(StepperMotor::FWD);
- //wait_ms(DELAY_1);
-
- speed1 = motor1->get_speed();
-
- printf("speed1 =%d \r\n\n", speed1);
- printf("U =%f \r\n\n", u);
-
- }
- else{ if(u<0.65) {
+ //printf("X =%i \r\n\n", flagX1);
+ // printf("Y =%i \r\n\n", flagY1);
+
+ // Movimentando eixo Z cima caso botão 1 apertado
+ if (zu >0.7) {
+ motor3->run(StepperMotor::FWD);
+ //wait_ms(DELAY_1);
+ speed3 = motor3->get_speed();
+ //wait_ms(DELAY_1);
+ }
- motor1->run(StepperMotor::BWD);
- // wait_ms(DELAY_1);
-
- speed1 = motor1->get_speed();
+ // Parando eixo Z caso botão 1 e 2 liberado
+ if (zu< 0.7 && zd < 0.7){
+ motor3->hard_stop();
+ // wait_ms(DELAY_1);
+ speed3 = 0;
+ //wait_ms(DELAY_1);
+ }
+
+// Movimentando eixo Z baixo caso botão 2 apertado
+ if (zd >0.7) {
+ motor3->run(StepperMotor::BWD);
+ //wait_ms(DELAY_1);
+ speed3 = motor3->get_speed();
+ //wait_ms(DELAY_1);
+ }
- printf("speed1 =%d \r\n\n", speed1);
-
- printf("U =%f \r\n\n", u);
-
+ // Parando eixo Z caso botão 1 e 2 liberado
+ if (zu< 0.7 && zd < 0.7){
+ motor3->hard_stop();
+ // wait_ms(DELAY_1);
+ speed3 = 0;
+ //wait_ms(DELAY_1);
+ }
+
+
+// Movimentando eixo Y duas direções com joystick
+ //if (flagY1 == 0 ) {
+
+// Movimentando eixo Y fwd
+ if (u>0.820) {
+ motor1->run(StepperMotor::FWD);
+ //wait_ms(DELAY_1);
+ speed1 = motor1->get_speed();
+ //wait_ms(DELAY_1);
+ }
+// Movimentando eixo Y bwd
+ else{ if(u<0.65) {
+ motor1->run(StepperMotor::BWD);
+ //wait_ms(DELAY_1);
+ speed1 = motor1->get_speed();
+ // wait_ms(DELAY_1);
}
- else
- motor1->hard_stop();
- // wait_ms(DELAY_1);
-
+// parando eixo Y
+ else
+ motor1->hard_stop();
+ //wait_ms(DELAY_1);
speed1 = 0;
- printf("speed1 =%d \r\n\n", speed1);
-
- printf("U =%f \r\n\n", u);
-
- }
-
- if (w>0.76) {
-
+ //wait_ms(DELAY_1);
+ //}
+ }
+
+// Movimentando eixo X duas direções com joystick
+ //if (flagX1 == 0) {
+// Movimentando eixo Y fwd
+ if (w>0.80) {
motor2->run(StepperMotor::BWD);
- speed2 = motor2->get_speed();
-
+ speed2 = motor2->get_speed();
printf("speed2 =%d \r\n\n", speed2);
printf("W =%f \r\n\n", w);
-
- }
- else{ if(w<0.67) {
-
-
+ }
+// Movimentando eixo Y Bwd
+ else{ if(w<0.60) {
motor2->run(StepperMotor::FWD);
- speed2 = motor2->get_speed();
-
+ speed2 = motor2->get_speed();
printf("speed2 =%d \r\n\n", speed2);
printf("W =%f \r\n\n", w);
-
- }
- else
+ }
+// parando eixo X
+ else
motor2->hard_stop();
speed2 = 0;
printf("speed2 =%d \r\n\n", speed2);
- printf("W =%f \r\n\n", w);
-
}
-
-
}
}
\ No newline at end of file