Revision d607fcef

View differences:

devices/DiWheelDrive/DiWheelDrive.cpp
2 2
#include "hal.h"
3 3
#include "qei.h"
4 4
#include "DiWheelDrive.h"
5
#include <chprintf.h>
5 6

  
6 7

  
7 8
#include <global.hpp>
......
118 119
        return RDY_OK;
119 120
      }
120 121
      break;
121
    case CAN::SET_LINE_FOLLOW_STRATEGY:
122
    case CAN::SET_LINE_FOLLOW_MSG:
123
      chprintf((BaseSequentialStream*) &SD1, "Received Strategy!\n");
122 124
      if (frame->DLC == 1) {
123 125
        global.lfStrategy = frame->data8[0];
126
        global.msgReceived = true;
124 127
        return RDY_OK;
125 128
      }
126 129
      break;
......
246 249
  frame.DLC = 6;
247 250
  this->transmitMessage(&frame);
248 251

  
252
  
253

  
249 254
  // Send the board ID (board ID of DiWheelDrive = Robot ID)
250 255
  if (this->bcCounter % 10 == 0) {
251 256
    frame.SID = 0;
devices/DiWheelDrive/global.hpp
206 206
  // 3 DOCKING
207 207
  int lfStrategy = 3;
208 208

  
209
  // CAN communication line:
210
  // Will be set to true when message was received 
211
  bool msgReceived = false; 
209 212
// Debugging stuff --------------
210 213
  int forwardSpeed = 10;
211 214
  int enableRecord = 0;
212 215

  
216
  bool triggerCan = false;
217
  uint8_t strategyTest = 0;
218

  
213 219
  
214 220

  
215 221
  // Buffer for sensor values
devices/DiWheelDrive/main.cpp
1170 1170

  
1171 1171

  
1172 1172
void setGlobalStrategy(BaseSequentialStream *chp, int argc, char *argv[]){
1173
  uint8_t strategy = 0;
1173 1174
  if(argc == 1){
1174
  
1175
    global.lfStrategy = atoi(argv[0]); 
1175
    strategy = atoi(argv[0]); 
1176 1176
  }
1177
  // send over can
1178
  global.strategyTest = strategy;
1179
  global.triggerCan = true;
1177 1180
}
1178 1181

  
1182

  
1183

  
1179 1184
void getPosition(BaseSequentialStream *chp, int argc, char *argv[]){
1180 1185
  types::position pos = global.odometry.getPosition();
1181 1186
  chprintf(chp, "Start: Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d\n", global.startPos.x, global.startPos.y, global.startPos.f_x, global.startPos.f_y, global.startPos.f_z);
devices/DiWheelDrive/userthread.cpp
15 15
  CHARGING
16 16
};
17 17

  
18
enum msg_content{
19
  STOP, 
20
  START,
21
  EDGE_LEFT,
22
  EDGE_RIGHT,
23
  FUZZY,
24
  DOCKING
25
};
26

  
18 27
// User thread state:
19 28
states utState = states::FOLLOW_LINE;
20 29

  
......
147 156
  int rpmSpeed[2] = {0};
148 157
  int stop[2] = {0};
149 158
  int proxyWheelThresh = 18000;
159
  LineFollowStrategy lStrategy;
150 160
  for (uint8_t led = 0; led < 8; ++led) {
151 161
    global.robot.setLightColor(led, Color(Color::BLACK));
152 162
  }
......
184 194
        this->sleep(1000);
185 195
        lightAllLeds(Color::BLACK);
186 196
        running = !running;
197
    // If message was received handle it here:
198
    } else if(global.msgReceived){
199

  
200
      global.msgReceived = false;
201
      running = true;
202
      switch(global.lfStrategy){
203
        case msg_content::START:
204
          break;
205
        case msg_content::STOP:
206
          running = false;
207
          break;
208
        case msg_content::EDGE_RIGHT:
209
          lStrategy = LineFollowStrategy::EDGE_RIGHT;
210
          break;
211
        case  msg_content::EDGE_LEFT:
212
          lStrategy = LineFollowStrategy::EDGE_LEFT;
213
          break;
214
        case msg_content::FUZZY:
215
          lStrategy = LineFollowStrategy::FUZZY;
216
          break;
217
        case msg_content::DOCKING:
218
          utState = states::OCCUPY;
219
          lStrategy = LineFollowStrategy::EDGE_RIGHT;
220
          break;
221
        default:
222
          break;
223
      }
224
    
187 225
    }
226

  
188 227
    if(running){
189 228
      switch(utState){
190 229
        case states::FOLLOW_LINE:{
191
          LineFollowStrategy lStrategy = determineStrategy();
230
          // LineFollowStrategy lStrategy = determineStrategy();
192 231
          if(lStrategy == LineFollowStrategy::DOCKING){
193 232
            utState = states::OCCUPY;
194 233
            break;
......
230 269
          }
231 270

  
232 271
          // Set correct following
233
          lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
272
          // lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
234 273
          lf.followLine(rpmSpeed);
235 274
          setRpmSpeed(rpmSpeed);
236 275
          
......
240 279
          break;
241 280
        }
242 281
        case states::CHARGING:{
282
          setRpmSpeed(stop);
243 283
          break;
244 284
        }
245 285
        default:{

Also available in: Unified diff