Skip to content

Commit

Permalink
added two GM3508 on remote controll
Browse files Browse the repository at this point in the history
  • Loading branch information
@Raventhatfly
Raventhatfly committed Jul 11, 2023
1 parent 9cbdcb7 commit b127a9f
Show file tree
Hide file tree
Showing 4 changed files with 41 additions and 57 deletions.
11 changes: 8 additions & 3 deletions dev/application/vehicles/dart/can_motor_config.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,16 +5,21 @@
#include "can_motor_config.h"

CANMotorBase CANMotorCFG::CANMotorProfile[MOTOR_COUNT] = {
{CANMotorBase::can_channel_1, 0x206, CANMotorBase::GM6020, 1419}
{CANMotorBase::can_channel_1, 0x206, CANMotorBase::GM6020, 1419},
{CANMotorBase::can_channel_1, 0x201, CANMotorBase::M3508, 0},
{CANMotorBase::can_channel_1, 0x202, CANMotorBase::M3508, 0},
};

PIDController::pid_params_t CANMotorCFG::a2vParams[MOTOR_COUNT] = {
{20, 0.0f, 500, 000, 550}
{20, 0.0f, 500, 000, 550},
{10, 0, 0.2, 100, 500},
{10, 0, 0.2, 100, 500},
};

PIDController::pid_params_t CANMotorCFG::v2iParams[MOTOR_COUNT] = {
{15.0f,0.0f,5.0f,5000,30000.0},

{26.0f, 0.1f, 0.02f, 2000.0, 6000.0},
{26.0f, 0.1f, 0.02f, 2000.0, 6000.0},
};

bool CANMotorCFG::enable_a2v[MOTOR_COUNT] {false};
Expand Down
4 changes: 3 additions & 1 deletion dev/application/vehicles/dart/can_motor_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,9 @@
class CANMotorCFG {
public:
enum motor_id_t {
MOTOR_ONE,
YAW,
STORE_ENERGY_LEFT,
STORE_ENERGY_RIGHT,
MOTOR_COUNT
};
static CANMotorBase CANMotorProfile[MOTOR_COUNT];
Expand Down
49 changes: 10 additions & 39 deletions dev/application/vehicles/dart/main_dart.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,59 +5,25 @@
#include "hal.h"
#include "can_motor_controller.h"
#include "can_motor_interface.h"
#include "chprintf.h"
#include "shell.h"
#include "remote_interpreter.h"
#include "user_dart.h"
#include "led.h"
#include "referee_interface.h"

CANInterface can1(&CAND1);
CANInterface can2(&CAND2);

//class remoteThread:public BaseStaticThread<512>{
//private:
// void main() final;
// CANMotorFeedback feedback;
//}remote_thread;
//
//void remoteThread::main(){
// setName("RemoteControl");
// static float angle;
// feedback = CANMotorIF::motor_feedback[CANMotorCFG::MOTOR_ONE];
// angle = feedback.accumulate_angle();
// while(!shouldTerminate()){
// chSysLock();
// if(Remote::rc.s1 == Remote::S_UP){
// CANMotorCFG::enable_v2i[CANMotorCFG::MOTOR_ONE] = false;
// CANMotorCFG::enable_a2v[CANMotorCFG::MOTOR_ONE] = false;
// CANMotorController::set_target_current(CANMotorCFG::MOTOR_ONE,0);
// }else if(Remote::rc.s1 == Remote::S_MIDDLE){
// CANMotorCFG::enable_v2i[CANMotorCFG::MOTOR_ONE] = true;
// CANMotorCFG::enable_a2v[CANMotorCFG::MOTOR_ONE] = false;
// CANMotorController::set_target_vel(CANMotorCFG::MOTOR_ONE, 200*Remote::rc.ch0);
// }else if(Remote::rc.s1 == Remote::S_DOWN){
// CANMotorCFG::enable_v2i[CANMotorCFG::MOTOR_ONE] = true;
// CANMotorCFG::enable_a2v[CANMotorCFG::MOTOR_ONE] = true;
// CANMotorController::set_target_angle(CANMotorCFG::MOTOR_ONE,angle);
// }
//
// chSysUnlock();
// feedback = CANMotorIF::motor_feedback[CANMotorCFG::MOTOR_ONE];
// LOG("initial angle: %f, current angle: %f\r\n",angle,feedback.accumulate_angle());
// chThdSleepMilliseconds(100);
// }
//}

class printThread: public BaseStaticThread<512>{
private:
CANMotorFeedback feedback;
void main() final{
setName("print");
while(!shouldTerminate()){
feedback = CANMotorIF::motor_feedback[CANMotorCFG::MOTOR_ONE];
feedback = CANMotorIF::motor_feedback[CANMotorCFG::YAW];
LOG("%f\n\r",feedback.accumulate_angle());
chThdSleepMilliseconds(500);
}

}
}print_thread;

Expand All @@ -66,12 +32,17 @@ int main(){
chibios_rt::System::init();
Shell::start(LOWPRIO);
Remote::start();

LED::led_on(0); // number 0 led turned on now
can1.start(NORMALPRIO+3);
can2.start(NORMALPRIO+4);
LED::led_on(1);// number 1 led turned on now

/// Setup Referee
Referee::init();

UserDart::start(NORMALPRIO-1);
// remote_thread.start(NORMALPRIO-1);
CANMotorController::start(HIGHPRIO-1,HIGHPRIO-2,&can1,&can2);
// print_thread.start(HIGHPRIO);
chThdSleepSeconds(5);

#if CH_CFG_NO_IDLE_THREAD // see chconf.h for what this #define means
Expand Down
34 changes: 20 additions & 14 deletions dev/application/vehicles/dart/user_dart.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,26 +10,32 @@ void UserDart::start(tprio_t user_thd_prio) {
void UserDart::UserThread::main(){
setName("RemoteControl");
static float angle;
feedback = CANMotorIF::motor_feedback[CANMotorCFG::MOTOR_ONE];
feedback = CANMotorIF::motor_feedback[CANMotorCFG::YAW];
angle = feedback.accumulate_angle();
while(!shouldTerminate()){
chSysLock();
if(Remote::rc.s1 == Remote::S_UP){
CANMotorCFG::enable_v2i[CANMotorCFG::MOTOR_ONE] = false;
CANMotorCFG::enable_a2v[CANMotorCFG::MOTOR_ONE] = false;
CANMotorController::set_target_current(CANMotorCFG::MOTOR_ONE,0);
}else if(Remote::rc.s1 == Remote::S_MIDDLE){
CANMotorCFG::enable_v2i[CANMotorCFG::MOTOR_ONE] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::MOTOR_ONE] = false;
CANMotorController::set_target_vel(CANMotorCFG::MOTOR_ONE, 200*Remote::rc.ch0);
}else if(Remote::rc.s1 == Remote::S_DOWN){
CANMotorCFG::enable_v2i[CANMotorCFG::MOTOR_ONE] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::MOTOR_ONE] = true;
CANMotorController::set_target_angle(CANMotorCFG::MOTOR_ONE,angle);
if(Remote::rc.s1 == Remote::S_UP){ // Forced Relax Mode
CANMotorCFG::enable_v2i[CANMotorCFG::YAW] = false;
CANMotorCFG::enable_a2v[CANMotorCFG::YAW] = false;
CANMotorController::set_target_current(CANMotorCFG::YAW,0);
}else if(Remote::rc.s1 == Remote::S_MIDDLE){ // Remote contorl mode
CANMotorCFG::enable_v2i[CANMotorCFG::YAW] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::YAW] = false;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_LEFT] = false;
CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = false;
CANMotorController::set_target_vel(CANMotorCFG::YAW, 200*Remote::rc.ch2);
CANMotorController::set_target_vel(CANMotorCFG::STORE_ENERGY_LEFT,500*Remote::rc.ch3);
CANMotorController::set_target_vel(CANMotorCFG::STORE_ENERGY_RIGHT,500*Remote::rc.ch3);
}else if(Remote::rc.s1 == Remote::S_DOWN){ // PC control mode, goes back to initial encoder angle
CANMotorCFG::enable_v2i[CANMotorCFG::YAW] = true;
CANMotorCFG::enable_a2v[CANMotorCFG::YAW] = true;
CANMotorController::set_target_angle(CANMotorCFG::YAW,angle);
}

chSysUnlock();
feedback = CANMotorIF::motor_feedback[CANMotorCFG::MOTOR_ONE];
feedback = CANMotorIF::motor_feedback[CANMotorCFG::YAW];
LOG("initial angle: %f, current angle: %f\r\n",angle,feedback.accumulate_angle());
chThdSleepMilliseconds(100);
}
Expand Down

0 comments on commit b127a9f

Please sign in to comment.