mukuyo hirohashi
/
Rasp_serial
raspberry pi to stm32f303k8 for serial communication
Diff: Drive/Drive.cpp
- Revision:
- 0:7bc24e8d1591
diff -r 000000000000 -r 7bc24e8d1591 Drive/Drive.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Drive/Drive.cpp Sun May 02 15:01:33 2021 +0000 @@ -0,0 +1,67 @@ +#include "Drive.hpp" +#define LED_PIN PF_0 + +Drive::Drive(PinName CAN_TX, PinName CAN_RX): Rasp(USBTX,USBRX), canMBED(CAN_TX, CAN_RX), LED(LED_PIN), servo(PA_8){ + initCAN(); + //can_Timer.attach(callback(this,&UDP::can_send), 16ms); + count = 0; +} + +void Drive::setVelocity(){ + //LED=0; + for(int i = 0; i < 4; i++) + { + M[i] = 0; + Rasp::get(M[i], i); + } + dribble_set(); + calcWheelVelocity(); + setMotVel(); + can_send(); +} + +void Drive::dribble_set(){ + if(dribble_power > 0){ + dribble_power = 0.25; + } + servo = dribble_power; +} + +void Drive::calcWheelVelocity(){ + + float max; + double plus; + + order.Mot0Order.Vel_short = M[0]; + order.Mot1Order.Vel_short = M[1]; + order.Mot2Order.Vel_short = M[2]; + order.Mot3Order.Vel_short = M[3]; + /*order.Mot0Order.Vel_short = -100; + order.Mot1Order.Vel_short = -100; + order.Mot2Order.Vel_short = 100; + order.Mot3Order.Vel_short = 100;*/ +} + +void Drive::setMotVel(){ + send_motvel_data[0] = (char)order.Mot0Order.Vel_char.L; + send_motvel_data[1] = (char)order.Mot0Order.Vel_char.H; + send_motvel_data[2] = (char)order.Mot1Order.Vel_char.L; + send_motvel_data[3] = (char)order.Mot1Order.Vel_char.H; + send_motvel_data[4] = (char)order.Mot2Order.Vel_char.L; + send_motvel_data[5] = (char)order.Mot2Order.Vel_char.H; + send_motvel_data[6] = (char)order.Mot3Order.Vel_char.L; + send_motvel_data[7] = (char)order.Mot3Order.Vel_char.H; +} + +void Drive::initCAN(){ + canMBED.frequency(100000); + +} + +void Drive::can_send(){ + canMBED.write(CANMessage(0x1AA,send_motvel_data,8)); +} + +int Drive::masuo_send(){ + return 1; +} \ No newline at end of file