Team DIANA
BASE
Dependencies: X-NUCLEO-IHM05A1
Diff: main.cpp
- Revision:
- 25:281c8e913db4
- Parent:
- 24:37f139e067b2
- Child:
- 26:44175c51a820
--- a/main.cpp Wed Jun 12 15:53:00 2019 +0000
+++ b/main.cpp Wed Jun 12 16:10:21 2019 +0000
@@ -1,209 +1,152 @@
#include "mbed.h"
-#include "L6206.h"
+#include "L6208.h"
+
+#define VREFA_PWM_PIN D3
+#define VREFB_PWM_PIN D9
-#define MAX_MOTOR (2)
+l6208_init_t init =
+{
+ 8000, //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)
+ 8000, //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)
+ 8000, //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
+ FAST_DECAY, //Decay mode via enum motorDecayMode_t
+ 0, //Dwelling time in ms
+ FALSE, //Automatic HIZ STOP
+ 100000 //VREFA and VREFB PWM frequency (Hz)
+};
-static volatile uint16_t gLastError;
-static volatile uint8_t gStep = 0;
+Thread canrxa;
+
+// Utility
+InterruptIn button(USER_BUTTON);
+DigitalOut led(LED1);
+
+// Motor Control
+L6208 *motor;
+
+InterruptIn end0(PC_10, PullUp);
+InterruptIn end1(PC_11, PullUp);
+InterruptIn enc(PC_12, PullUp);
int current_pose = 0;
-int speed = 0;
-
-L6206_init_t init =
-{
- L6206_CONF_PARAM_PARALLE_BRIDGES,
- {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B},
- {100,100,100,100},
- {FORWARD,FORWARD,BACKWARD,FORWARD},
- {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
- {FALSE,FALSE}
-};
-
-L6206 *motor;
-InterruptIn my_button_irq(USER_BUTTON); /* User button on Nucleo board */
-Thread canrxa;
-
-//Utility
-InterruptIn button(USER_BUTTON);
-DigitalOut led(LED1); //Change?
-
+int pose = 0;
void motor_error_handler(uint16_t error)
{
printf("ERROR: Motor Runtime\n\r");
- while(1){};
+ while(1){}
}
void motor_zero()
{
- motor->run(0, BDCMotor::FWD);
- motor->run(1, BDCMotor::FWD);
+ motor->run(StepperMotor::FWD);
}
-void button_int_handler(unsigned int motorId)
+void button_int_handler()
{
- printf("MOTOR SPEED: %d\n\r", motor->get_speed(motorId));
+ printf("POSITION: %d\n\r", motor->get_position());
motor_zero();
}
-void my_error_handler(uint16_t error)
-{
- /* Backup error number */
- gLastError = error;
-
- /* Enter your own code here */
-}
-
-void my_flag_irq_handler(void)
+void end0_int_handler()
{
- /* Code to be customised */
- /************************/
- /* Get the state of bridge A */
- uint16_t bridgeState = motor->get_bridge_status(0);
-
- if (bridgeState == 0) {
- if ((motor->get_device_state(0) != INACTIVE)||
- (motor->get_device_state(1) != INACTIVE)) {
- /* Bridge A was disabling due to overcurrent or over temperature */
- /* When at least on of its motor was running */
- my_error_handler(0XBAD0);
- }
- }
-
- /* Get the state of bridge B */
- bridgeState = motor->get_bridge_status(1);
-
- if (bridgeState == 0) {
- if ((motor->get_device_state(2) != INACTIVE)||
- (motor->get_device_state(3) != INACTIVE)) {
- /* Bridge A was disabling due to overcurrent or over temperature */
- /* When at least on of its motor was running */
- my_error_handler(0XBAD1);
- }
- }
-}
-void end0_int_handler(unsigned int motorId)
-{
- printf("END0: Pressed\n\rSPEED: %d\n\r", motor->get_speed(motorId));
+ printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
}
void end1_int_handler()
{
- motor->hard_stop(0);
- motor->hard_stop(1); //or hard_hiz(); for disabling the bridge?
+ motor->hard_stop();
- motor->run(0, BDCMotor::BWD);
- motor->run(1, BDCMotor::BWD);
+ 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;
+}
-// CAN, to revise
+// CAN
CAN can1(PB_12, PB_13); // RX, TX
CANMessage messageIn;
CANMessage messageOut;
-
int filter = can1.filter(0x010, 0x4FF, CANStandard);
void canrx()
{
while(1)
{
- if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==6))) //Primo motore
+ if(can1.read(messageIn,filter)&&messageIn.id==0x010)
{
- speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno
- float speedMap=(speed-127)/127*100;
- printf("CAN: mess %d\n\r", speedMap, "\% ");
+ pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
+ printf("CAN: mess %d\n\r", pose);
- //CAN MESSAGE WITH SPEED TO REVISE
- //Ci sarebbe anche il set speed.
- if (speedMap == 0)
+
+ if (pose == 1)
{
- motor->set_speed(0,0); //There's no soft stop. could it work like this?
- //current_speed= motor->get_speed(0); We could do lie this?
- //motor->go_to(current_pose);
+ motor->run(StepperMotor::FWD);
}
- else if (speedMap>0)
- {
- motor->run(0,BDCMotor::FWD);
- motor->set_speed(0,(unsigned int) speedMap);
- }
- else if (speedMap<0)
+ else if (pose == 0)
{
- motor->run(0,BDCMotor::BWD);
- motor->set_speed(0,(unsigned int) -speedMap);
+ motor->soft_stop();
+ current_pose= motor->get_position();
+ motor->go_to(current_pose);
+ }
+ else if (pose == 2)
+ {
+ motor->run(StepperMotor::BWD);
}
else
{
- motor->set_speed(0,0); //Riportare errore? NO
- }
- }
- else if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==7))) //Secondo motore
- {
- speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno
- float speedMap=(speed-127)/127*100;
- printf("CAN: mess %d\n\r", speedMap, "\% ");
-
- //CAN MESSAGE WITH SPEED TO REVISE
- //Ci sarebbe anche il set speed.
- if (speedMap == 0)
- {
- motor->set_speed(1,0); //There's no soft stop. could it work like this?
- //current_speed= motor->get_speed(0); We could do lie this?
- //motor->go_to(current_pose);
+ motor->soft_stop();
+ current_pose= motor->get_position();
+ motor->go_to(current_pose);
}
- else if (speedMap>0)
- {
- motor->run(1,BDCMotor::FWD);
- motor->set_speed(1,(unsigned int) speedMap);
- }
- else if (speedMap<0)
- {
- motor->run(1,BDCMotor::BWD);
- motor->set_speed(1,(unsigned int) -speedMap);
- }
- else
- {
- motor->set_speed(0,0); //Riportare errore? NO
- }
- }
+
+
+ // motor->go_to(pose);
+ }
+ }
}
-//Aggiungere un get speed,FATTO ci sarebbe anche il set_speed
- //Dentro la libreria controllare se c'è controllo di Duty cycle FATTO
- //motor->go_to(current_pose);
+
/* Main ----------------------------------------------------------------------*/
int main()
{
can1.frequency(125000);
- messageIn.format=CANExtended;
- messageOut.format=CANExtended;
- // Motor Initialization
-#ifdef TARGET_STM32F429
- motor = new L6206(D2, A4, PB_4, PC_7, PA_15, PB_3);
-#else
- motor = new L6206(D2, A4, D5, D4, A0, A1);
-#endif
-
- if (motor->init(&init) != COMPONENT_OK)
+ // 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: vvMotor Init\n\r");
exit(EXIT_FAILURE);
}
- motor->attach_flag_interrupt(my_flag_irq_handler);
- motor->attach_error_handler(my_error_handler);
-
-
+ 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);
+ motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
printf("DONE: Motor Init\n\r");
// CAN Initialization