Diff: Drive/Drive.cpp

Revision:

0:7bc24e8d1591

--- /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