USNA WSE ES456 / Madpulse

Interface Library for Madpulse RC Car

Dependents:   Madpulse_Speed_Control_temp Madpulse_Speed_Control_Students

madpulse.cpp@1:6bba42f337e6, 2017-11-15 (annotated)

Committer:
jdawkins
Date:
Wed Nov 15 18:36:09 2017 +0000
Revision:
1:6bba42f337e6
Parent:
0:5e55308aa7a6 
Commit for class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdawkins 0:5e55308aa7a6 1 #include "mbed.h"
jdawkins 0:5e55308aa7a6 2 #include "madpulse.h"
jdawkins 0:5e55308aa7a6 3 #define DT 0.02
jdawkins 0:5e55308aa7a6 4 MadPulse::MadPulse(uint8_t id,Serial *port,BNO055 *imu, ServoIn *CH1,ServoIn *CH2, InterruptIn *irq, ServoOut *Steer, ServoOut *Throttle):
jdawkins 0:5e55308aa7a6 5 id(id),
jdawkins 0:5e55308aa7a6 6 _imu(imu),
jdawkins 0:5e55308aa7a6 7 _port(port),
jdawkins 0:5e55308aa7a6 8 _wheel_irq(irq),
jdawkins 0:5e55308aa7a6 9 _CH1(CH1),
jdawkins 0:5e55308aa7a6 10 _CH2(CH2),
jdawkins 0:5e55308aa7a6 11 _Steer(Steer),
jdawkins 0:5e55308aa7a6 12 _Throttle(Throttle)
jdawkins 0:5e55308aa7a6 13 {
jdawkins 0:5e55308aa7a6 14 ti.start(); //Start Local Timer
jdawkins 0:5e55308aa7a6 15
jdawkins 0:5e55308aa7a6 16 _wheel_irq->rise(this, &MadPulse::wheelCallback); // Callback to Interrupt for Wheel Sensor
jdawkins 0:5e55308aa7a6 17
jdawkins 0:5e55308aa7a6 18 if(_imu->check()) {
jdawkins 0:5e55308aa7a6 19
jdawkins 0:5e55308aa7a6 20 printf("BNO055 connected\r\n");
jdawkins 0:5e55308aa7a6 21 _imu->setmode(OPERATION_MODE_CONFIG);
jdawkins 0:5e55308aa7a6 22 _imu->SetExternalCrystal(1);
jdawkins 0:5e55308aa7a6 23 _imu->setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 0:5e55308aa7a6 24 _imu->set_angle_units(RADIANS);
jdawkins 0:5e55308aa7a6 25 _imu->set_accel_units(MPERSPERS);
jdawkins 0:5e55308aa7a6 26 _imu->set_anglerate_units(RAD_PER_SEC);
jdawkins 0:5e55308aa7a6 27 _imu->setoutputformat(WINDOWS);
jdawkins 0:5e55308aa7a6 28 _imu->set_mapping(2);
jdawkins 0:5e55308aa7a6 29
jdawkins 0:5e55308aa7a6 30 // imu_LED = 1; // turns on IMU light when calibration is successful
jdawkins 0:5e55308aa7a6 31
jdawkins 0:5e55308aa7a6 32 }
jdawkins 0:5e55308aa7a6 33 // _port->attach(this,&MadPulse::rxCallback);
jdawkins 0:5e55308aa7a6 34
jdawkins 0:5e55308aa7a6 35 };
jdawkins 0:5e55308aa7a6 36 void MadPulse::sampleData(){
jdawkins 0:5e55308aa7a6 37 getAccel();
jdawkins 0:5e55308aa7a6 38 getGyro();
jdawkins 0:5e55308aa7a6 39 getMag();
jdawkins 0:5e55308aa7a6 40 getEuler();
jdawkins 0:5e55308aa7a6 41 getSpeed();
jdawkins 0:5e55308aa7a6 42
jdawkins 0:5e55308aa7a6 43 //printf("Wheel Freq: %f\n\r",wheel_freq);
jdawkins 0:5e55308aa7a6 44
jdawkins 0:5e55308aa7a6 45 }
jdawkins 0:5e55308aa7a6 46 void MadPulse::getAccel(){
jdawkins 0:5e55308aa7a6 47 _imu->get_accel();
jdawkins 0:5e55308aa7a6 48 accel.x = _imu->accel.x;
jdawkins 0:5e55308aa7a6 49 accel.y = _imu->accel.y;
jdawkins 0:5e55308aa7a6 50 accel.z = _imu->accel.z;
jdawkins 0:5e55308aa7a6 51 }
jdawkins 0:5e55308aa7a6 52 void MadPulse::getGyro(){
jdawkins 0:5e55308aa7a6 53 _imu->get_gyro();
jdawkins 0:5e55308aa7a6 54 gyro.x = _imu->gyro.x;
jdawkins 0:5e55308aa7a6 55 gyro.y = _imu->gyro.y;
jdawkins 0:5e55308aa7a6 56 gyro.z = _imu->gyro.z;
jdawkins 0:5e55308aa7a6 57 }
jdawkins 0:5e55308aa7a6 58 void MadPulse::getMag(){
jdawkins 0:5e55308aa7a6 59 _imu->get_mag();
jdawkins 0:5e55308aa7a6 60 mag.x = _imu->mag.x;
jdawkins 0:5e55308aa7a6 61 mag.y = _imu->mag.y;
jdawkins 0:5e55308aa7a6 62 mag.z = _imu->mag.z;
jdawkins 0:5e55308aa7a6 63 // printf("Mag:%.1f,%.1f,%.1f\n\r",_imu->mag.x,_imu->mag.y,_imu->mag.z);
jdawkins 0:5e55308aa7a6 64 }
jdawkins 0:5e55308aa7a6 65 void MadPulse::getEuler(){
jdawkins 0:5e55308aa7a6 66 _imu->get_angles();
jdawkins 0:5e55308aa7a6 67 euler.roll = _imu->euler.roll;
jdawkins 0:5e55308aa7a6 68 euler.pitch = _imu->euler.pitch;
jdawkins 0:5e55308aa7a6 69 euler.yaw = _imu->euler.yaw;
jdawkins 0:5e55308aa7a6 70 }
jdawkins 0:5e55308aa7a6 71
jdawkins 0:5e55308aa7a6 72
jdawkins 0:5e55308aa7a6 73 void MadPulse::getSpeed(){
jdawkins 0:5e55308aa7a6 74
jdawkins 0:5e55308aa7a6 75 float dt_loc = ti.read()-t_ws;
jdawkins 0:5e55308aa7a6 76
jdawkins 0:5e55308aa7a6 77
jdawkins 0:5e55308aa7a6 78 if(dt_loc > 0.1){
jdawkins 0:5e55308aa7a6 79 wheel_freq = 0;
jdawkins 0:5e55308aa7a6 80 }
jdawkins 0:5e55308aa7a6 81 /* if( (dt < 0.05) && (dt > 0.002)) {
jdawkins 0:5e55308aa7a6 82 // wheel_spd = (1/CTS_REV)/(dt_ws); // 0.036 is the wheel radius v = omega*r
jdawkins 0:5e55308aa7a6 83 //wheel_freq = 1/dt_ws;
jdawkins 0:5e55308aa7a6 84 } else {
jdawkins 0:5e55308aa7a6 85
jdawkins 0:5e55308aa7a6 86
jdawkins 0:5e55308aa7a6 87 }*/
jdawkins 0:5e55308aa7a6 88
jdawkins 0:5e55308aa7a6 89
jdawkins 0:5e55308aa7a6 90
jdawkins 0:5e55308aa7a6 91 }
jdawkins 0:5e55308aa7a6 92
jdawkins 0:5e55308aa7a6 93 void MadPulse::resetEnc(){
jdawkins 0:5e55308aa7a6 94 counts = 0;
jdawkins 0:5e55308aa7a6 95 }
jdawkins 0:5e55308aa7a6 96 void MadPulse::wheelCallback()
jdawkins 0:5e55308aa7a6 97 {
jdawkins 0:5e55308aa7a6 98 dt = ti.read()-t_ws;
jdawkins 0:5e55308aa7a6 99
jdawkins 0:5e55308aa7a6 100 // if(dt < 0.0001){
jdawkins 0:5e55308aa7a6 101 if(dt < 0.0001){
jdawkins 0:5e55308aa7a6 102 wheel_freq = 1/dt_old;
jdawkins 0:5e55308aa7a6 103 dt_old = dt_old;
jdawkins 0:5e55308aa7a6 104
jdawkins 0:5e55308aa7a6 105 }else{
jdawkins 0:5e55308aa7a6 106 wheel_freq = 1/dt;
jdawkins 0:5e55308aa7a6 107 dt_old = dt;
jdawkins 0:5e55308aa7a6 108 }
jdawkins 0:5e55308aa7a6 109
jdawkins 0:5e55308aa7a6 110 if(wheel_freq >200){
jdawkins 0:5e55308aa7a6 111 wheel_freq=wheel_freq_old;
jdawkins 0:5e55308aa7a6 112 }
jdawkins 0:5e55308aa7a6 113 wheel_freq_old = wheel_freq;
jdawkins 0:5e55308aa7a6 114 /* if(dt > 0.1){
jdawkins 0:5e55308aa7a6 115 wheel_freq = 0;
jdawkins 0:5e55308aa7a6 116 }*/
jdawkins 0:5e55308aa7a6 117
jdawkins 0:5e55308aa7a6 118
jdawkins 0:5e55308aa7a6 119 // Hall effect speed sensor callback
jdawkins 0:5e55308aa7a6 120 wheel_LED = !wheel_LED;
jdawkins 0:5e55308aa7a6 121 counts++; // Iterate tick counter
jdawkins 0:5e55308aa7a6 122 dt_ws = ti.read()-t_ws;
jdawkins 0:5e55308aa7a6 123 t_ws= ti.read();
jdawkins 0:5e55308aa7a6 124 } // end void he_callback
jdawkins 0:5e55308aa7a6 125
jdawkins 0:5e55308aa7a6 126 void MadPulse::setSteer(float str){
jdawkins 0:5e55308aa7a6 127 str = saturateCmd(str);
jdawkins 0:5e55308aa7a6 128
jdawkins 0:5e55308aa7a6 129 _Steer->write((int)((str*500.0)+1500.0)); // Write Steering Pulse
jdawkins 0:5e55308aa7a6 130 }
jdawkins 0:5e55308aa7a6 131 void MadPulse::setThrottle(float thr){
jdawkins 0:5e55308aa7a6 132 thr = saturateVal(thr,0.3,-0.3);
jdawkins 0:5e55308aa7a6 133 _Throttle->write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
jdawkins 0:5e55308aa7a6 134 }
jdawkins 0:5e55308aa7a6 135
jdawkins 0:5e55308aa7a6 136
jdawkins 0:5e55308aa7a6 137 uint8_t MadPulse::sendMessage(Serial *obj,char* msg,uint8_t len)
jdawkins 0:5e55308aa7a6 138 {
jdawkins 0:5e55308aa7a6 139 int i;
jdawkins 0:5e55308aa7a6 140 for(i=0; i<len; i++) {
jdawkins 0:5e55308aa7a6 141 obj->printf("%c",msg[i]);
jdawkins 0:5e55308aa7a6 142 }
jdawkins 0:5e55308aa7a6 143 obj->printf("\r\n");
jdawkins 0:5e55308aa7a6 144 return i;
jdawkins 0:5e55308aa7a6 145 }
jdawkins 0:5e55308aa7a6 146
jdawkins 0:5e55308aa7a6 147 void MadPulse::rxCallback(){
jdawkins 0:5e55308aa7a6 148
jdawkins 0:5e55308aa7a6 149 char c = _port->getc();
jdawkins 0:5e55308aa7a6 150 // pc.printf("%s",c);
jdawkins 0:5e55308aa7a6 151 if(c == 'H' || c == 'h') {
jdawkins 0:5e55308aa7a6 152 _port->printf("Command List...\r\n");
jdawkins 0:5e55308aa7a6 153 _port->printf("d - set open loop duty cycle\r\n");
jdawkins 0:5e55308aa7a6 154 _port->printf("r - run open loop control with current duty cycle\r\n");
jdawkins 0:5e55308aa7a6 155 _port->printf("h - display this prompt and exit\r\n");
jdawkins 0:5e55308aa7a6 156 _port->printf("t - set duration of test\r\n\n\n");
jdawkins 0:5e55308aa7a6 157 _port->printf("Enter Command\r\n");
jdawkins 0:5e55308aa7a6 158 }
jdawkins 0:5e55308aa7a6 159 else if(c == 'R' || c == 'r') {
jdawkins 0:5e55308aa7a6 160 // auto_flag = 1;
jdawkins 0:5e55308aa7a6 161 auto_LED = 1;
jdawkins 0:5e55308aa7a6 162 }
jdawkins 0:5e55308aa7a6 163 else if(c == 'S' || c == 's') {
jdawkins 0:5e55308aa7a6 164 // auto_flag = 0;
jdawkins 0:5e55308aa7a6 165 auto_LED = 0;
jdawkins 0:5e55308aa7a6 166 }
jdawkins 0:5e55308aa7a6 167 else {
jdawkins 0:5e55308aa7a6 168
jdawkins 0:5e55308aa7a6 169 }
jdawkins 0:5e55308aa7a6 170 }
jdawkins 0:5e55308aa7a6 171
jdawkins 0:5e55308aa7a6 172
jdawkins 0:5e55308aa7a6 173 int MadPulse::parseMessage(char * msg)
jdawkins 0:5e55308aa7a6 174 {
jdawkins 0:5e55308aa7a6 175
jdawkins 0:5e55308aa7a6 176 if(!strncmp(msg, "$POS", 4)) {
jdawkins 0:5e55308aa7a6 177 // int arg1;
jdawkins 0:5e55308aa7a6 178 float arg2,arg3,arg4;
jdawkins 0:5e55308aa7a6 179
jdawkins 0:5e55308aa7a6 180 sscanf(msg,"$POS,%f,%f,%f\n",&arg2,&arg3,&arg4);
jdawkins 0:5e55308aa7a6 181
jdawkins 0:5e55308aa7a6 182 pos.x = arg2;
jdawkins 0:5e55308aa7a6 183 pos.y = arg3;
jdawkins 0:5e55308aa7a6 184 pos.z = arg4;
jdawkins 0:5e55308aa7a6 185
jdawkins 0:5e55308aa7a6 186 // pc.printf("idm: %d n:%f e:%f d:%f",arg1,pos_n,pos_e,pos_d);
jdawkins 0:5e55308aa7a6 187
jdawkins 0:5e55308aa7a6 188 return 1;
jdawkins 0:5e55308aa7a6 189 } //emd of $CMD
jdawkins 0:5e55308aa7a6 190
jdawkins 0:5e55308aa7a6 191 if(!strncmp(msg, "$RUN", 4)) {
jdawkins 0:5e55308aa7a6 192
jdawkins 0:5e55308aa7a6 193 return 1;
jdawkins 0:5e55308aa7a6 194 }
jdawkins 0:5e55308aa7a6 195 if(!strncmp(msg, "$STOP", 4)) {
jdawkins 0:5e55308aa7a6 196
jdawkins 0:5e55308aa7a6 197 return 1;
jdawkins 0:5e55308aa7a6 198 }
jdawkins 0:5e55308aa7a6 199
jdawkins 0:5e55308aa7a6 200
jdawkins 0:5e55308aa7a6 201 return 0;
jdawkins 0:5e55308aa7a6 202
jdawkins 0:5e55308aa7a6 203 }