Diff: main.cpp

Revision:

17:dc1b04f0b55d

Parent:

16:1dc772fba09f

Child:

18:65707db67191

--- a/main.cpp	Fri Dec 07 17:40:15 2018 +0000
+++ b/main.cpp	Thu Feb 07 09:13:56 2019 +0000
@@ -1,21 +1,16 @@
+#include "mbed.h"
+#include "L6208.h"
 
-/* Includes ------------------------------------------------------------------*/
-/* mbed specific header files. */
-#include "mbed.h"
-/* Component specific header files. */
-#include "L6208.h"
-/* Definitions ---------------------------------------------------------------*/
 #define VREFA_PWM_PIN D3
 #define VREFB_PWM_PIN D9
-/* Variables -----------------------------------------------------------------*/
-/* Initialization parameters of the motor connected to the expansion board. */
+
 l6208_init_t init =
 {
-  1500,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
+  2000,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
   80,              //Acceleration current torque in % (from 0 to 100)
-  1500,            //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
+  2000,            //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
   80,              //Deceleration current torque in % (from 0 to 100)
-  1000,            //Running speed in step/s or (1/16)th step/s for microstep modes
+  6000,            //Running speed in step/s or (1/16)th step/s for microstep modes
   80,              //Running current torque in % (from 0 to 100)
   40,              //Holding current torque in % (from 0 to 100)
   STEP_MODE_1_16,  //Step mode via enum motorStepMode_t
@@ -25,121 +20,121 @@
   100000           //VREFA and VREFB PWM frequency (Hz)
 };
 
-/* Motor Control Component. */
+
+// Utility
+InterruptIn button(USER_BUTTON);
+DigitalOut led(LED1);
+
+// Motor Control
 L6208 *motor;
-DigitalIn button (USER_BUTTON);
-DigitalIn end(PC_10);
-DigitalOut led(LED1);
-/* Functions -----------------------------------------------------------------*/
+
+InterruptIn end0(PC_10, PullUp);
+InterruptIn end1(PC_11, PullUp);
+InterruptIn enc(PC_12, PullUp);
+
+int current_pose = 0;
+int pose = 0;
 
-/**
- * @brief  This is an example of user handler for the flag interrupt.
- * @param  None
- * @retval None
- * @note   If needed, implement it, and then attach and enable it:
- *           + motor->attach_flag_irq(&my_flag_irq_handler);
- *           + motor->enable_flag_irq();
- *         To disable it:
- *           + motor->DisbleFlagIRQ();
- */
-void my_flag_irq_handler(void)
+void motor_error_handler(uint16_t error)
+{
+  printf("ERROR: Motor Runtime\n\r");
+  while(1){} 
+}
+
+void button_int_handler()
 {
-  printf("    WARNING: \"FLAG\" interrupt triggered:\r\n");
-  motor->disable();
-  printf("    Motor disabled.\r\n\n");
+    printf("POSITION: %d\n\r", motor->get_position());
+    
+    motor->go_to(-160000);
+}
+
+void end0_int_handler()
+{
+  printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
 }
 
-/**
- * @brief  This is an example of error handler.
- * @param[in] error Number of the error
- * @retval None
- * @note   If needed, implement it, and then attach it:
- *           + motor->attach_error_handler(&my_error_handler);
- */
-void my_error_handler(uint16_t error)
+void end1_int_handler()
+{
+  motor->hard_stop();
+
+  motor->run(StepperMotor::BWD);
+
+  printf("END1: Pressed\n\r");
+}
+
+void motor_set_home()
+{
+  motor->hard_stop();
+  motor->set_home();
+  motor->go_to(0);
+  
+  current_pose = 0;
+  pose = 0;
+}
+
+void motor_zero()
 {
-  /* Printing to the console. */
-  printf("Error %d detected\r\n\n", error);
+  motor->run(StepperMotor::BWD);
+}
+
+// CAN
+CAN can1(PB_8, PB_9);     // RX, TX
+
+CANMessage messageIn;
+CANMessage messageOut;
+
+int filter = can1.filter(0x020, 0x4FF, CANStandard);
+
+void can_rx_handler()
+{
+  printf("CAN: Rec\n\r");
   
-  /* Infinite loop */
-  while (true) {
-  }    
+  if(can1.read(messageIn, filter))
+  {
+    printf("CAN: Rec %d\n\r", (messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24)));
+  }
 }
 
+
+
 /* Main ----------------------------------------------------------------------*/
 
 int main()
 {
-
-//----- Initialization 
-  /* Initializing Motor Control Component. */
+  // Motor Initialization 
   motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
-  if (motor->init(&init) != COMPONENT_OK) {
-    printf("error init\n\r");
+  
+  if (motor->init(&init) != COMPONENT_OK)
+  {
+    printf("ERROR: Motor Init\n\r");
     exit(EXIT_FAILURE);
   }
 
-  /* Attaching and enabling an interrupt handler. */
-  motor->attach_flag_irq(&my_flag_irq_handler);
-  motor->enable_flag_irq();
-    
-  /* Attaching an error handler */
-  motor->attach_error_handler(&my_error_handler);
- 
-  motor->set_step_mode(StepperMotor::STEP_MODE_1_8);
-
-  /* Set speed, acceleration and deceleration to scale with microstep mode */
- //motor->set_max_speed(2000);
- //motor->set_acceleration(2400);
- //motor->set_deceleration(2400);
- motor->set_max_speed(motor->get_max_speed()<<2);
-  motor->set_acceleration(motor->get_acceleration()<<2);
-  motor->set_deceleration(motor->get_deceleration()<<2);
-  printf(" init\n\r");
-  end.mode(PullUp);
-  /* Request to go position 9000 (quarter steps) */
-   motor->run(StepperMotor::BWD);
-  while(1)
-    {
-   //  motor->wait_while_active();
-     printf("while");
-      if(end) break;
-    }
-  motor->hard_stop(); 
-  led=1;
+  motor->attach_error_handler(&motor_error_handler);
+  
+  end0.rise(&end0_int_handler);
+  end1.rise(&end1_int_handler);
+  end1.fall(&motor_set_home);
+  
+  button.rise(&button_int_handler);
+  
+  printf("DONE: Motor Init\n\r");
   
-  int pos=motor->get_position();
-  printf("pos:%d\n\r", pos);
-  motor->go_to(pos+200);
-  motor->wait_while_active();
-  led=0;
-  wait(0.5);
-
-  motor->set_home();
-  wait(1);
-
-  motor->go_to(1000<<4);
-  motor->wait_while_active();
- 
-  while (true)
-{
-  motor->go_to(500<<4);
-  motor->wait_while_active();
- 
-   motor->go_to(0);
-   motor->wait_while_active();
-   while (button)
+  // CAN Initialization
+  can1.reset();
+  can1.frequency(500000);
+  can1.attach(&can_rx_handler);
+  
+  printf("DONE: CAN Init\n\r");
+  
+  motor->run(StepperMotor::FWD);
+  
+  printf("Running!\n\r");
+  
+  while(true)
   {
-  
-   }
-}
- 
-  
-    /* Waiting while the motor is active. */
-    motor->wait_while_active();
-  
-    wait_ms(5000);  
-  motor->disable();
+    wait(1000);
+  }
 }